From 71c87bb49e85b82a57c91e32229b5c6c5e170383 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 7 Nov 2014 10:37:40 +1100 Subject: [PATCH 001/293] px4io: fixed errno returns to be negative --- src/drivers/px4io/px4io.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f02ba62c8a2..b31d7bbfa469 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); @@ -2592,9 +2592,9 @@ 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; + return -E2BIG; } /* copy values to registers in IO */ From 61f51a69ab6fa9f5bafc1befec94383f54cc4df1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 7 Nov 2014 20:00:40 +1100 Subject: [PATCH 002/293] 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 000000000000..973eb775df70 --- /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 000000000000..6bb53c71a5b6 --- /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 be4f68e7a4fb7af09b28a1ebaf8e30a5cfe4b1be Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Nov 2014 15:48:33 +1100 Subject: [PATCH 003/293] 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 a1a02965f43b..f0c2cfd26d6e 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 4d207146aa4adc477651ecaa987f109492c19f1d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 3 Nov 2014 11:24:28 +1100 Subject: [PATCH 004/293] 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 3a1e1b7b51a9..6db6713c4e59 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -159,13 +159,15 @@ Airspeed::init() 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 a639a2cfb2972ff4072cea18eba3c2a4cc53cb21 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Nov 2014 09:22:24 +1100 Subject: [PATCH 005/293] 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 | 74 +++++++++++++++++++------------------ 1 file changed, 38 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b31d7bbfa469..58390ba4c3b9 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; - - } 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; - } + if (have_armed == OK) { + if (armed.armed) { + set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } else { + clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } - /* 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; - } + if (armed.lockdown && !_lockdown_override) { + set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } else { + clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } - // 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; - // } + /* 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; + } - if (armed.ready_to_arm) { - set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + // 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; + // } - } else { - clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + 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 d9d1573085369073ff34c7caddeaf306daac178f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 28 Nov 2014 09:15:03 +1100 Subject: [PATCH 006/293] lsm303d: only use GPIO_EXTI_ACCEL_DRDY when available --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 01c89192a7f8..30e55e61601e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1522,11 +1522,13 @@ LSM303D::measure() // read a value and then miss the next value. // Note that DRDY is not available when the lsm303d is // connected on the external bus +#ifdef GPIO_EXTI_ACCEL_DRDY if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { perf_count(_accel_reschedules); hrt_call_delay(&_accel_call, 100); return; } +#endif if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { perf_count(_reg1_resets); reset(); From 5b7fa4400deca70d16f8c6497d0f0959a43ba340 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 4 Dec 2014 07:41:59 +1100 Subject: [PATCH 007/293] makefiles: removed stray spaces --- makefiles/toolchain_gnu-arm-eabi.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 84e5cd08a221..718aa74be5a0 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -109,10 +109,10 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections # enable precise stack overflow tracking # note - requires corresponding support in NuttX From 8cae3db67c9166691b79fb1e42abdd92e4697209 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 4 Dec 2014 07:42:30 +1100 Subject: [PATCH 008/293] makefiles: make it easier to use ccache for build on windows --- makefiles/toolchain_gnu-arm-eabi.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 718aa74be5a0..8ba4f429e66f 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -227,7 +227,7 @@ DEP_INCLUDES = $(subst .o,.d,$(OBJS)) define COMPILE @$(ECHO) "CC: $1" @$(MKDIR) -p $(dir $2) - $(Q) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 + $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 endef # Compile C++ source $1 to $2 @@ -236,7 +236,7 @@ endef define COMPILEXX @$(ECHO) "CXX: $1" @$(MKDIR) -p $(dir $2) - $(Q) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 + $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 endef # Assemble $1 into $2 From c93002faa61205ebcebd12e3a3a7a85e1fa8fc14 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Dec 2014 14:29:58 +1100 Subject: [PATCH 009/293] defconfig: raise CONFIG_NFILE_DESCRIPTORS to 50 we were running out of file descriptors with the new battery monitoring --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index edf49b26e827..f82be0b468f3 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -404,7 +404,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=38 +CONFIG_NFILE_DESCRIPTORS=50 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e31f40cd2a76..d31648694b1c 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -438,7 +438,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=38 +CONFIG_NFILE_DESCRIPTORS=50 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 From e6ca00690a0174cd03c8a9149f0098e3bafc6664 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Sat, 20 Dec 2014 15:21:16 +1100 Subject: [PATCH 010/293] MPU6000: Add regdump command Add mpu6000 regdump command for debugging mpu6000. --- src/drivers/mpu6000/mpu6000.cpp | 44 +++++++++++++++++++++++++++++++-- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index a95e041a116c..c9c27892f4ad 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -194,6 +194,8 @@ class MPU6000 : public device::SPI */ void print_info(); + void print_registers(); + protected: virtual int probe(); @@ -1414,6 +1416,21 @@ MPU6000::print_info() _gyro_reports->print_info("gyro queue"); } +void +MPU6000::print_registers() +{ + printf("MPU6000 registers\n"); + for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) { + uint8_t v = read_reg(reg); + printf("%02x:%02x ",(unsigned)reg, (unsigned)v); + if ((reg - (MPUREG_PRODUCT_ID-1)) % 13 == 0) { + printf("\n"); + } + } + printf("\n"); +} + + MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : CDev("MPU6000_gyro", path), _parent(parent), @@ -1479,6 +1496,7 @@ void start(bool, enum Rotation); void test(bool); void reset(bool); void info(bool); +void regdump(bool); void usage(); /** @@ -1654,10 +1672,26 @@ info(bool external_bus) exit(0); } +/** + * Dump the register information + */ +void +regdump(bool external_bus) +{ + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + printf("regdump @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_registers(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -1714,5 +1748,11 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(verb, "info")) mpu6000::info(external_bus); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) + mpu6000::regdump(external_bus); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } From a462073083ef1e41876a92b673be5f98001f1727 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 24 Dec 2014 12:56:10 -0800 Subject: [PATCH 011/293] mpu6000: monitor some key registers for correct values this will catch both bad SPI bus comms and a sensor that has been reset causing incorrect configuration. --- src/drivers/mpu6000/mpu6000.cpp | 142 +++++++++++++++++++++++++++++--- 1 file changed, 132 insertions(+), 10 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index c9c27892f4ad..3d720cd94b17 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -231,8 +231,11 @@ class MPU6000 : public device::SPI perf_counter_t _gyro_reads; perf_counter_t _sample_perf; perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; perf_counter_t _good_transfers; + uint8_t _register_wait; + math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; @@ -242,6 +245,14 @@ class MPU6000 : public device::SPI enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define MPU6000_NUM_CHECKED_REGISTERS 4 + static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + /** * Start automatic measurement. */ @@ -281,7 +292,7 @@ class MPU6000 : public device::SPI * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg, uint32_t speed=MPU6000_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); /** @@ -303,6 +314,14 @@ class MPU6000 : public device::SPI */ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + /** + * Write a register in the MPU6000, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + /** * Set the MPU6000 measurement range. * @@ -347,11 +366,26 @@ class MPU6000 : public device::SPI */ void _set_sample_rate(uint16_t desired_sample_rate_hz); + /* + check that key registers still have the right value + */ + void check_registers(void); + /* do not allow to copy this class due to pointer data members */ MPU6000(const MPU6000&); MPU6000 operator=(const MPU6000&); }; +/* + list of registers that will be checked in check_registers(). Note + that MPUREG_PRODUCT_ID must be first in the list. + */ +const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, + MPUREG_GYRO_CONFIG, + MPUREG_ACCEL_CONFIG, + MPUREG_USER_CTRL }; + + /** * Helper class implementing the gyro driver node. */ @@ -407,14 +441,17 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), + _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), + _register_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _rotation(rotation) + _rotation(rotation), + _checked_next(0) { // disable debug() calls _debug_enabled = false; @@ -460,6 +497,7 @@ MPU6000::~MPU6000() perf_free(_accel_reads); perf_free(_gyro_reads); perf_free(_bad_transfers); + perf_free(_bad_registers); perf_free(_good_transfers); } @@ -590,7 +628,7 @@ void MPU6000::reset() up_udelay(1000); // Disable I2C bus (recommended on datasheet) - write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); irqrestore(state); usleep(1000); @@ -605,7 +643,7 @@ void MPU6000::reset() _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); usleep(1000); // Gyro scale 2000 deg/s () - write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); + write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); usleep(1000); // correct gyro scale factors @@ -624,7 +662,7 @@ void MPU6000::reset() case MPU6000_REV_C5: // Accel scale 8g (4096 LSB/g) // Rev C has different scaling than rev D - write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); + write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); break; case MPU6000ES_REV_D6: @@ -639,7 +677,7 @@ void MPU6000::reset() // presumably won't have the accel scaling bug default: // Accel scale 8g (4096 LSB/g) - write_reg(MPUREG_ACCEL_CONFIG, 2 << 3); + write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); break; } @@ -684,6 +722,7 @@ MPU6000::probe() case MPU6000_REV_D9: case MPU6000_REV_D10: debug("ID 0x%02x", _product); + _checked_values[0] = _product; return OK; } @@ -734,7 +773,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) } else { filter = BITS_DLPF_CFG_2100HZ_NOLPF; } - write_reg(MPUREG_CONFIG, filter); + write_checked_reg(MPUREG_CONFIG, filter); } ssize_t @@ -1094,12 +1133,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) } uint8_t -MPU6000::read_reg(unsigned reg) +MPU6000::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; // general register transfer at low clock speed - set_frequency(MPU6000_LOW_BUS_SPEED); + set_frequency(speed); transfer(cmd, cmd, sizeof(cmd)); @@ -1144,6 +1183,17 @@ MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) write_reg(reg, val); } +void +MPU6000::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; imeasure(); } +void +MPU6000::check_registers(void) +{ + /* + we read the register at full speed, even though it isn't + listed as a high speed register. The low speed requirement + for some registers seems to be a propgation delay + requirement for changing sensor configuration, which should + not apply to reading a single register. It is also a better + test of SPI bus health to read at the same speed as we read + the data registers. + */ + uint8_t v; + if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. We skip zero as that is the PRODUCT_ID, which + is not writeable + */ + if (_checked_next != 0) { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + } +#if 0 + if (_register_wait == 0) { + ::printf("MPU6000: %02x:%02x should be %02x\n", + (unsigned)_checked_registers[_checked_next], + (unsigned)v, + (unsigned)_checked_values[_checked_next]); + } +#endif + _register_wait = 20; + } else { + _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; + } +} + void MPU6000::measure() { @@ -1254,6 +1350,8 @@ MPU6000::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + check_registers(); + // sensor transfer at high clock speed set_frequency(MPU6000_HIGH_BUS_SPEED); @@ -1292,6 +1390,14 @@ MPU6000::measure() } perf_count(_good_transfers); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. We still increment + // _good_transfers, but don't return any data yet + _register_wait--; + return; + } /* @@ -1321,7 +1427,12 @@ MPU6000::measure() * Adjust and scale results to m/s^2. */ grb.timestamp = arb.timestamp = hrt_absolute_time(); - grb.error_count = arb.error_count = 0; // not reported + + // report the error count as the sum of the number of bad + // transfers and bad register reads. This allows the higher + // level code to decide if it should use this sensor based on + // whether it has had failures + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1411,9 +1522,20 @@ MPU6000::print_info() perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); perf_print_counter(_good_transfers); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; i Date: Mon, 29 Dec 2014 11:03:08 +1100 Subject: [PATCH 012/293] lsm303d: replace old register checking with new check_registers() method this uses the same method as is now used in the MPU6000 to check that the sensor retains its correct values --- src/drivers/lsm303d/lsm303d.cpp | 345 ++++++++++++-------------------- 1 file changed, 123 insertions(+), 222 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 30e55e61601e..dbd5c1f4c3a9 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -77,10 +77,6 @@ #endif static const int ERROR = -1; -// enable this to debug the buggy lsm303d sensor in very early -// prototype pixhawk boards -#define CHECK_EXTREMES 0 - /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -241,16 +237,6 @@ class LSM303D : public device::SPI */ void print_registers(); - /** - * toggle logging - */ - void toggle_logging(); - - /** - * check for extreme accel values - */ - void check_extremes(const accel_report *arb); - protected: virtual int probe(); @@ -292,30 +278,25 @@ class LSM303D : public device::SPI perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; - perf_counter_t _reg1_resets; - perf_counter_t _reg7_resets; - perf_counter_t _extreme_values; perf_counter_t _accel_reschedules; + perf_counter_t _bad_registers; + + uint8_t _register_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; - // expceted values of reg1 and reg7 to catch in-flight - // brownouts of the sensor - uint8_t _reg1_expected; - uint8_t _reg7_expected; - - // accel logging - int _accel_log_fd; - bool _accel_logging_enabled; - uint64_t _last_extreme_us; - uint64_t _last_log_us; - uint64_t _last_log_sync_us; - uint64_t _last_log_reg_us; - uint64_t _last_log_alarm_us; enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define LSM303D_NUM_CHECKED_REGISTERS 6 + static const uint8_t _checked_registers[LSM303D_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + /** * Start automatic measurement. */ @@ -356,6 +337,11 @@ class LSM303D : public device::SPI */ static void mag_measure_trampoline(void *arg); + /** + * check key registers for correct values + */ + void check_registers(void); + /** * Fetch accel measurements from the sensor and update the report ring. */ @@ -407,6 +393,14 @@ class LSM303D : public device::SPI */ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + /** + * Write a register in the LSM303D, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + /** * Set the LSM303D accel measurement range. * @@ -468,6 +462,17 @@ class LSM303D : public device::SPI LSM303D operator=(const LSM303D&); }; +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG5, + ADDR_CTRL_REG6, + ADDR_CTRL_REG7 }; + /** * Helper class implementing the mag driver node. */ @@ -528,23 +533,14 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), - _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), - _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), + _bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")), + _register_wait(0), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _reg1_expected(0), - _reg7_expected(0), - _accel_log_fd(-1), - _accel_logging_enabled(false), - _last_extreme_us(0), - _last_log_us(0), - _last_log_sync_us(0), - _last_log_reg_us(0), - _last_log_alarm_us(0), - _rotation(rotation) + _rotation(rotation), + _checked_next(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; @@ -586,9 +582,7 @@ LSM303D::~LSM303D() /* delete the perf counter */ perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); - perf_free(_reg1_resets); - perf_free(_reg7_resets); - perf_free(_extreme_values); + perf_free(_bad_registers); perf_free(_accel_reschedules); } @@ -703,13 +697,12 @@ LSM303D::reset() disable_i2c(); /* enable accel*/ - _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; - write_reg(ADDR_CTRL_REG1, _reg1_expected); + write_checked_reg(ADDR_CTRL_REG1, + REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); /* enable mag */ - _reg7_expected = REG7_CONT_MODE_M; - write_reg(ADDR_CTRL_REG7, _reg7_expected); - write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 @@ -739,126 +732,12 @@ LSM303D::probe() /* verify that the device is attached and functioning */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - if (success) + if (success) { + _checked_values[0] = WHO_I_AM; return OK; - - return -EIO; -} - -#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log" - -/** - check for extreme accelerometer values and log to a file on the SD card - */ -void -LSM303D::check_extremes(const accel_report *arb) -{ - const float extreme_threshold = 30; - static bool boot_ok = false; - bool is_extreme = (fabsf(arb->x) > extreme_threshold && - fabsf(arb->y) > extreme_threshold && - fabsf(arb->z) > extreme_threshold); - if (is_extreme) { - perf_count(_extreme_values); - // force accel logging on if we see extreme values - _accel_logging_enabled = true; - } else { - boot_ok = true; - } - - if (! _accel_logging_enabled) { - // logging has been disabled by user, close - if (_accel_log_fd != -1) { - ::close(_accel_log_fd); - _accel_log_fd = -1; - } - return; - } - if (_accel_log_fd == -1) { - // keep last 10 logs - ::unlink(ACCEL_LOGFILE ".9"); - for (uint8_t i=8; i>0; i--) { - uint8_t len = strlen(ACCEL_LOGFILE)+3; - char log1[len], log2[len]; - snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i); - snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1)); - ::rename(log1, log2); - } - ::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1"); - - // open the new logfile - _accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666); - if (_accel_log_fd == -1) { - return; - } - } - - uint64_t now = hrt_absolute_time(); - // log accels at 1Hz - if (_last_log_us == 0 || - now - _last_log_us > 1000*1000) { - _last_log_us = now; - ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n", - (unsigned long long)arb->timestamp, - (double)arb->x, (double)arb->y, (double)arb->z, - (int)arb->x_raw, - (int)arb->y_raw, - (int)arb->z_raw, - (unsigned)boot_ok); - } - - const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, - ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, - ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, - ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, - ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, - ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, - ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, - ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, - ADDR_ACT_THS, ADDR_ACT_DUR, - ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, - ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I}; - uint8_t regval[sizeof(reglist)]; - for (uint8_t i=0; i 250*1000)) || - (now - _last_log_reg_us > 10*1000*1000)) { - _last_log_reg_us = now; - ::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time()); - for (uint8_t i=0; i 10*1000*1000) { - _last_log_sync_us = now; - ::fsync(_accel_log_fd); - } - - // play alarm every 10s if we have had an extreme value - if (perf_event_count(_extreme_values) != 0 && - (now - _last_log_alarm_us > 10*1000*1000)) { - _last_log_alarm_us = now; - int tfd = ::open(TONEALARM_DEVICE_PATH, 0); - if (tfd != -1) { - uint8_t tone = 3; - if (!is_extreme) { - tone = 3; - } else if (boot_ok) { - tone = 4; - } else { - tone = 5; - } - ::ioctl(tfd, TONE_SET_ALARM, tone); - ::close(tfd); - } - } + return -EIO; } ssize_t @@ -879,9 +758,6 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { -#if CHECK_EXTREMES - check_extremes(arb); -#endif ret += sizeof(*arb); arb++; } @@ -1262,6 +1138,17 @@ LSM303D::write_reg(unsigned reg, uint8_t value) transfer(cmd, nullptr, sizeof(cmd)); } +void +LSM303D::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; imag_measure(); } +void +LSM303D::check_registers(void) +{ + uint8_t v; + if ((v=read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. We skip zero as that is the WHO_AM_I, which + is not writeable + */ + if (_checked_next != 0) { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + } +#if 1 + if (_register_wait == 0) { + ::printf("LSM303D: %02x:%02x should be %02x\n", + (unsigned)_checked_registers[_checked_next], + (unsigned)v, + (unsigned)_checked_values[_checked_next]); + } +#endif + _register_wait = 20; + } + _checked_next = (_checked_next+1) % LSM303D_NUM_CHECKED_REGISTERS; +} + void LSM303D::measure() { @@ -1529,11 +1450,6 @@ LSM303D::measure() return; } #endif - if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { - perf_count(_reg1_resets); - reset(); - return; - } /* status register and data as read back from the device */ @@ -1552,6 +1468,16 @@ LSM303D::measure() /* start the performance counter */ perf_begin(_accel_sample_perf); + check_registers(); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. + _register_wait--; + perf_end(_accel_sample_perf); + return; + } + /* fetch data from the sensor */ memset(&raw_accel_report, 0, sizeof(raw_accel_report)); raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; @@ -1574,7 +1500,12 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); - accel_report.error_count = 0; // not reported + + // report the error count as the sum of the number of bad bad + // register reads. This allows the higher level code to decide + // if it should use this sensor based on whether it has had + // failures + accel_report.error_count = perf_event_count(_bad_registers); accel_report.x_raw = raw_accel_report.x; accel_report.y_raw = raw_accel_report.y; @@ -1613,12 +1544,6 @@ LSM303D::measure() void LSM303D::mag_measure() { - if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) { - perf_count(_reg7_resets); - reset(); - return; - } - /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -1693,8 +1618,19 @@ LSM303D::print_info() printf("accel reads: %u\n", _accel_read); printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); + perf_print_counter(_bad_registers); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; itoggle_logging(); - - exit(0); -} - void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2128,11 +2035,5 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) lsm303d::regdump(); - /* - * dump device registers - */ - if (!strcmp(verb, "logging")) - lsm303d::logging(); - - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', or 'regdump'"); } From 3c4f0a7a4032801f1e5d4afc76b0a924a0c4bd36 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 14:00:09 +1100 Subject: [PATCH 013/293] mpu6000: try resetting the mpu6000 up to 5 times this mirrors the ardupilot driver. We have seen situations where the mpu6000 on the Pixhawk comes up in SLEEP mode, despite a reset --- src/drivers/mpu6000/mpu6000.cpp | 62 +++++++++++++++++++++------------ 1 file changed, 39 insertions(+), 23 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 3d720cd94b17..eb9666dc5ac3 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -233,6 +233,7 @@ class MPU6000 : public device::SPI perf_counter_t _bad_transfers; perf_counter_t _bad_registers; perf_counter_t _good_transfers; + perf_counter_t _reset_retries; uint8_t _register_wait; @@ -248,7 +249,7 @@ class MPU6000 : public device::SPI // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define MPU6000_NUM_CHECKED_REGISTERS 4 +#define MPU6000_NUM_CHECKED_REGISTERS 5 static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -268,7 +269,7 @@ class MPU6000 : public device::SPI * * Resets the chip and measurements ranges, but not scale and offset. */ - void reset(); + int reset(); /** * Static trampoline from the hrt_call context; because we don't have a @@ -383,7 +384,8 @@ class MPU6000 : public device::SPI const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, - MPUREG_USER_CTRL }; + MPUREG_USER_CTRL, + MPUREG_PWR_MGMT_1}; /** @@ -443,6 +445,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), + _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -524,7 +527,8 @@ MPU6000::init() if (_gyro_reports == nullptr) goto out; - reset(); + if (reset() != OK) + goto out; /* Initialize offsets and scales */ _accel_scale.x_offset = 0; @@ -609,27 +613,39 @@ MPU6000::init() return ret; } -void MPU6000::reset() +int MPU6000::reset() { // if the mpu6000 is initialised after the l3gd20 and lsm303d // then if we don't do an irqsave/irqrestore here the mpu6000 // frequenctly comes up in a bad state where all transfers // come as zero - irqstate_t state; - state = irqsave(); - - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - up_udelay(10000); - - // Wake up device and select GyroZ clock. Note that the - // MPU6000 starts up in sleep mode, and it can take some time - // for it to come out of sleep - write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); - up_udelay(1000); + uint8_t tries = 5; + while (--tries != 0) { + irqstate_t state; + state = irqsave(); + + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + up_udelay(10000); + + // Wake up device and select GyroZ clock. Note that the + // MPU6000 starts up in sleep mode, and it can take some time + // for it to come out of sleep + write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + up_udelay(1000); + + // Disable I2C bus (recommended on datasheet) + write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + irqrestore(state); - // Disable I2C bus (recommended on datasheet) - write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); - irqrestore(state); + if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) { + break; + } + perf_count(_reset_retries); + usleep(2000); + } + if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) { + return -EIO; + } usleep(1000); @@ -698,6 +714,7 @@ void MPU6000::reset() // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); + return OK; } int @@ -913,8 +930,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCRESET: - reset(); - return OK; + return reset(); case SENSORIOCSPOLLRATE: { switch (arg) { @@ -1306,9 +1322,8 @@ MPU6000::check_registers(void) } #endif _register_wait = 20; - } else { - _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; } + _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; } void @@ -1524,6 +1539,7 @@ MPU6000::print_info() perf_print_counter(_bad_transfers); perf_print_counter(_bad_registers); perf_print_counter(_good_transfers); + perf_print_counter(_reset_retries); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); ::printf("checked_next: %u\n", _checked_next); From 7971140d32d8bf876bef56455a4d98f4e6e824b0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 14:18:10 +1100 Subject: [PATCH 014/293] l3gd20: added register checking this checks at runtime that key registers have correct values --- src/drivers/l3gd20/l3gd20.cpp | 146 ++++++++++++++++++++++++++++------ 1 file changed, 122 insertions(+), 24 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 82fa5cc6e71e..de9d9140f051 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -225,6 +225,9 @@ class L3GD20 : public device::SPI perf_counter_t _sample_perf; perf_counter_t _reschedules; perf_counter_t _errors; + perf_counter_t _bad_registers; + + uint8_t _register_wait; math::LowPassFilter2p _gyro_filter_x; math::LowPassFilter2p _gyro_filter_y; @@ -235,6 +238,14 @@ class L3GD20 : public device::SPI enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define L3GD20_NUM_CHECKED_REGISTERS 7 + static const uint8_t _checked_registers[L3GD20_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + /** * Start automatic measurement. */ @@ -266,6 +277,11 @@ class L3GD20 : public device::SPI */ static void measure_trampoline(void *arg); + /** + * check key registers for correct values + */ + void check_registers(void); + /** * Fetch measurements from the sensor and update the report ring. */ @@ -298,6 +314,14 @@ class L3GD20 : public device::SPI */ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + /** + * Write a register in the L3GD20, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + /** * Set the L3GD20 measurement range. * @@ -338,6 +362,18 @@ class L3GD20 : public device::SPI L3GD20 operator=(const L3GD20&); }; +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG3, + ADDR_CTRL_REG4, + ADDR_CTRL_REG5, + ADDR_FIFO_CTRL_REG }; + L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call{}, @@ -355,11 +391,14 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), + _bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")), + _register_wait(0), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _is_l3g4200d(false), - _rotation(rotation) + _rotation(rotation), + _checked_next(0) { // enable debug() calls _debug_enabled = true; @@ -389,6 +428,7 @@ L3GD20::~L3GD20() perf_free(_sample_perf); perf_free(_reschedules); perf_free(_errors); + perf_free(_bad_registers); } int @@ -448,29 +488,27 @@ L3GD20::probe() (void)read_reg(ADDR_WHO_AM_I); bool success = false; + uint8_t v = 0; - /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { - + /* verify that the device is attached and functioning, accept + * L3GD20, L3GD20H and L3G4200D */ + if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) { _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; - } - - - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { + } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) { _orientation = SENSOR_BOARD_ROTATION_180_DEG; success = true; - } - - /* Detect the L3G4200D used on AeroCore */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) { + } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) { + /* Detect the L3G4200D used on AeroCore */ _is_l3g4200d = true; _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; } - if (success) + if (success) { + _checked_values[0] = v; return OK; + } return -EIO; } @@ -672,6 +710,18 @@ L3GD20::write_reg(unsigned reg, uint8_t value) transfer(cmd, nullptr, sizeof(cmd)); } +void +L3GD20::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; iprint_info("report queue"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; i Date: Mon, 29 Dec 2014 16:15:41 +1100 Subject: [PATCH 015/293] mpu6000: added factory self-test function this follows the factory calibration self-test method in the datasheet to see if the sensor still has the same calibration it had in the factory --- src/drivers/mpu6000/mpu6000.cpp | 226 +++++++++++++++++++++++++++++--- 1 file changed, 208 insertions(+), 18 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index eb9666dc5ac3..2559c73e0582 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -113,6 +113,10 @@ #define MPUREG_FIFO_COUNTL 0x73 #define MPUREG_FIFO_R_W 0x74 #define MPUREG_PRODUCT_ID 0x0C +#define MPUREG_TRIM1 0x0D +#define MPUREG_TRIM2 0x0E +#define MPUREG_TRIM3 0x0F +#define MPUREG_TRIM4 0x10 // Configuration bits MPU 3000 and MPU 6000 (not revised)? #define BIT_SLEEP 0x40 @@ -121,6 +125,9 @@ #define MPU_CLK_SEL_PLLGYROX 0x01 #define MPU_CLK_SEL_PLLGYROZ 0x03 #define MPU_EXT_SYNC_GYROX 0x02 +#define BITS_GYRO_ST_X 0x80 +#define BITS_GYRO_ST_Y 0x40 +#define BITS_GYRO_ST_Z 0x20 #define BITS_FS_250DPS 0x00 #define BITS_FS_500DPS 0x08 #define BITS_FS_1000DPS 0x10 @@ -196,6 +203,13 @@ class MPU6000 : public device::SPI void print_registers(); + /** + * Test behaviour against factory offsets + * + * @return 0 on success, 1 on failure + */ + int factory_self_test(); + protected: virtual int probe(); @@ -254,6 +268,10 @@ class MPU6000 : public device::SPI uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; + // use this to avoid processing measurements when in factory + // self test + volatile bool _in_factory_test; + /** * Start automatic measurement. */ @@ -375,6 +393,24 @@ class MPU6000 : public device::SPI /* do not allow to copy this class due to pointer data members */ MPU6000(const MPU6000&); MPU6000 operator=(const MPU6000&); + +#pragma pack(push, 1) + /** + * Report conversation within the MPU6000, including command byte and + * interrupt status. + */ + struct MPUReport { + uint8_t cmd; + uint8_t status; + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + }; +#pragma pack(pop) }; /* @@ -454,7 +490,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), - _checked_next(0) + _checked_next(0), + _in_factory_test(false) { // disable debug() calls _debug_enabled = false; @@ -889,6 +926,152 @@ MPU6000::gyro_self_test() return 0; } + +/* + perform a self-test comparison to factory trim values. This takes + about 200ms and will return OK if the current values are within 14% + of the expected values + */ +int +MPU6000::factory_self_test() +{ + _in_factory_test = true; + uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG); + uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG); + const uint16_t repeats = 100; + int ret = OK; + + // gyro self test has to be done at 250DPS + write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); + + struct MPUReport mpu_report; + float accel_baseline[3]; + float gyro_baseline[3]; + float accel[3]; + float gyro[3]; + float accel_ftrim[3]; + float gyro_ftrim[3]; + + // get baseline values without self-test enabled + set_frequency(MPU6000_HIGH_BUS_SPEED); + + memset(accel_baseline, 0, sizeof(accel_baseline)); + memset(gyro_baseline, 0, sizeof(gyro_baseline)); + memset(accel, 0, sizeof(accel)); + memset(gyro, 0, sizeof(gyro)); + + for (uint8_t i=0; i>3)&0x1C) | ((trims[3]>>4)&0x03); + atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03); + atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03); + gtrim[0] = trims[0] & 0x1F; + gtrim[1] = trims[1] & 0x1F; + gtrim[2] = trims[2] & 0x1F; + + // convert factory trims to right units + for (uint8_t i=0; i<3; i++) { + accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f); + gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1); + } + // Y gyro trim is negative + gyro_ftrim[1] *= -1; + + for (uint8_t i=0; i<3; i++) { + float diff = accel[i]-accel_baseline[i]; + float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i]; + ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*accel_baseline[i]), + (int)(1000*accel[i]), + (int)(1000*diff), + (int)(1000*accel_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + for (uint8_t i=0; i<3; i++) { + float diff = gyro[i]-gyro_baseline[i]; + float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; + ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*gyro_baseline[i]), + (int)(1000*gyro[i]), + (int)(1000*(gyro[i]-gyro_baseline[i])), + (int)(1000*gyro_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + + write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); + write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); + + _in_factory_test = false; + + return ret; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -1329,24 +1512,12 @@ MPU6000::check_registers(void) void MPU6000::measure() { -#pragma pack(push, 1) - /** - * Report conversation within the MPU6000, including command byte and - * interrupt status. - */ - struct MPUReport { - uint8_t cmd; - uint8_t status; - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - } mpu_report; -#pragma pack(pop) + if (_in_factory_test) { + // don't publish any data while in factory test mode + return; + } + struct MPUReport mpu_report; struct Report { int16_t accel_x; int16_t accel_y; @@ -1635,6 +1806,7 @@ void test(bool); void reset(bool); void info(bool); void regdump(bool); +void factorytest(bool); void usage(); /** @@ -1826,6 +1998,21 @@ regdump(bool external_bus) exit(0); } +/** + * Dump the register information + */ +void +factorytest(bool external_bus) +{ + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + (*g_dev_ptr)->factory_self_test(); + + exit(0); +} + void usage() { @@ -1892,5 +2079,8 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) mpu6000::regdump(external_bus); + if (!strcmp(verb, "factorytest")) + mpu6000::factorytest(external_bus); + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } From 1c176089d8952af3f0cbe27d056610d132881e62 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 20:44:05 +1100 Subject: [PATCH 016/293] mpu6000: monitor some more registers --- src/drivers/mpu6000/mpu6000.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 2559c73e0582..91eb40b96282 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -263,7 +263,7 @@ class MPU6000 : public device::SPI // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define MPU6000_NUM_CHECKED_REGISTERS 5 +#define MPU6000_NUM_CHECKED_REGISTERS 9 static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -418,10 +418,14 @@ class MPU6000 : public device::SPI that MPUREG_PRODUCT_ID must be first in the list. */ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, + MPUREG_PWR_MGMT_1, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_USER_CTRL, - MPUREG_PWR_MGMT_1}; + MPUREG_INT_ENABLE, + MPUREG_INT_PIN_CFG, + MPUREG_SMPLRT_DIV }; /** From c53a0a67e0e888bb42898591e47b0d3944e23380 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 20:44:46 +1100 Subject: [PATCH 017/293] mpu6000: added "mpu6000 testerror" command used to generate a error case for reset testing --- src/drivers/mpu6000/mpu6000.cpp | 47 +++++++++++++++++++++++++++++++-- 1 file changed, 45 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 91eb40b96282..8dfac9859c8c 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -210,6 +210,9 @@ class MPU6000 : public device::SPI */ int factory_self_test(); + // deliberately cause a sensor error + void test_error(); + protected: virtual int probe(); @@ -1072,10 +1075,31 @@ MPU6000::factory_self_test() write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); _in_factory_test = false; + if (ret == OK) { + ::printf("PASSED\n"); + } return ret; } + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +MPU6000::test_error() +{ + _in_factory_test = true; + // deliberately trigger an error. This was noticed during + // development as a handy way to test the reset logic + uint8_t data[16]; + memset(data, 0, sizeof(data)); + transfer(data, data, sizeof(data)); + ::printf("error triggered\n"); + print_registers(); + _in_factory_test = false; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -1810,6 +1834,7 @@ void test(bool); void reset(bool); void info(bool); void regdump(bool); +void testerror(bool); void factorytest(bool); void usage(); @@ -2002,6 +2027,21 @@ regdump(bool external_bus) exit(0); } +/** + * deliberately produce an error to test recovery + */ +void +testerror(bool external_bus) +{ + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + (*g_dev_ptr)->test_error(); + + exit(0); +} + /** * Dump the register information */ @@ -2020,7 +2060,7 @@ factorytest(bool external_bus) void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2086,5 +2126,8 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(verb, "factorytest")) mpu6000::factorytest(external_bus); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + if (!strcmp(verb, "testerror")) + mpu6000::testerror(external_bus); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'"); } From 7cf0fe521eaa5afc19c48af806d9349014a69599 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 20:45:31 +1100 Subject: [PATCH 018/293] mpu6000: wait for 10ms after a full reset this prevents the mpu6000 getting in a really weird state! --- src/drivers/mpu6000/mpu6000.cpp | 31 ++++++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 8dfac9859c8c..558006d0dfd7 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -253,6 +253,7 @@ class MPU6000 : public device::SPI perf_counter_t _reset_retries; uint8_t _register_wait; + uint64_t _reset_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -490,6 +491,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), + _reset_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -749,9 +751,9 @@ int MPU6000::reset() usleep(1000); // INT CFG => Interrupt on Data Ready - write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready + write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready usleep(1000); - write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read + write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read usleep(1000); // Oscillator set @@ -800,7 +802,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) uint8_t div = 1000 / desired_sample_rate_hz; if(div>200) div=200; if(div<1) div=1; - write_reg(MPUREG_SMPLRT_DIV, div-1); + write_checked_reg(MPUREG_SMPLRT_DIV, div-1); _sample_rate = 1000 / div; } @@ -937,7 +939,7 @@ MPU6000::gyro_self_test() /* perform a self-test comparison to factory trim values. This takes about 200ms and will return OK if the current values are within 14% - of the expected values + of the expected values (as per datasheet) */ int MPU6000::factory_self_test() @@ -1518,10 +1520,20 @@ MPU6000::check_registers(void) /* try to fix the bad register value. We only try to fix one per loop to prevent a bad sensor hogging the - bus. We skip zero as that is the PRODUCT_ID, which - is not writeable + bus. */ - if (_checked_next != 0) { + if (_checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + // after doing a reset we need to wait a long + // time before we do any other register writes + // or we will end up with the mpu6000 in a + // bizarre state where it has all correct + // register values but large offsets on the + // accel axes + _reset_wait = hrt_absolute_time() + 10000; + } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); } #if 0 @@ -1545,6 +1557,11 @@ MPU6000::measure() return; } + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + struct MPUReport mpu_report; struct Report { int16_t accel_x; From bc0962e6f56e166d5eb48e61d478e39fddd0e7f0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Dec 2014 08:41:00 +1100 Subject: [PATCH 019/293] mpu6000: make register fixup much closer to a reset() this may help automatic reset on the faulty boards --- src/drivers/mpu6000/mpu6000.cpp | 42 +++++++++++++++++++++++++++++---- 1 file changed, 37 insertions(+), 5 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 558006d0dfd7..e7e88bb82b11 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -254,6 +254,7 @@ class MPU6000 : public device::SPI uint8_t _register_wait; uint64_t _reset_wait; + uint64_t _printf_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -423,13 +424,14 @@ class MPU6000 : public device::SPI */ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, MPUREG_PWR_MGMT_1, - MPUREG_CONFIG, + MPUREG_USER_CTRL, + MPUREG_SMPLRT_DIV, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, - MPUREG_USER_CTRL, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG, - MPUREG_SMPLRT_DIV }; + MPUREG_INT_PIN_CFG }; + /** @@ -492,6 +494,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), _reset_wait(0), + _printf_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -1522,7 +1525,7 @@ MPU6000::check_registers(void) fix one per loop to prevent a bad sensor hogging the bus. */ - if (_checked_next == 0) { + if (_register_wait == 0 || _checked_next == 0) { // if the product_id is wrong then reset the // sensor completely write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); @@ -1533,8 +1536,37 @@ MPU6000::check_registers(void) // register values but large offsets on the // accel axes _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + _reset_wait = hrt_absolute_time() + 1000; + if (_checked_next == 1 && hrt_absolute_time() > _printf_wait) { + /* + rather bizarrely this printf() seems + to be critical for successfully + resetting the sensor. If you take + this printf out then all the + registers do get successfully reset, + but the sensor ends up with a large + fixed offset on the + accelerometers. Try a up_udelay() + of various sizes instead of a + printf() doesn't work. That makes no + sense at all, and investigating why + this is would be worthwhile. + + The rate limit on the printf to 5Hz + prevents this from causing enough + delays in interrupt context to cause + the vehicle to not be able to fly + correctly. + */ + _printf_wait = hrt_absolute_time() + 200*1000UL; + ::printf("Setting %u %02x to %02x\n", + (unsigned)_checked_next, + (unsigned)_checked_registers[_checked_next], + (unsigned)_checked_values[_checked_next]); + } } #if 0 if (_register_wait == 0) { From 31b8ed10e9948e8dc8ba263d420a10ce6e02d7ac Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Dec 2014 13:14:29 +1100 Subject: [PATCH 020/293] l3gd20: added "l3gd20 regdump" command --- src/drivers/l3gd20/l3gd20.cpp | 43 +++++++++++++++++++++++++++++++++-- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index de9d9140f051..63974a7df046 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -200,6 +200,9 @@ class L3GD20 : public device::SPI */ void print_info(); + // print register dump + void print_registers(); + protected: virtual int probe(); @@ -1074,6 +1077,20 @@ L3GD20::print_info() } } +void +L3GD20::print_registers() +{ + printf("L3GD20 registers\n"); + for (uint8_t reg=0; reg<=0x40; reg++) { + uint8_t v = read_reg(reg); + printf("%02x:%02x ",(unsigned)reg, (unsigned)v); + if ((reg+1) % 16 == 0) { + printf("\n"); + } + } + printf("\n"); +} + int L3GD20::self_test() { @@ -1109,6 +1126,7 @@ void start(bool external_bus, enum Rotation rotation); void test(); void reset(); void info(); +void regdump(); /** * Start the driver. @@ -1247,10 +1265,25 @@ info() exit(0); } +/** + * Dump the register information + */ +void +regdump(void) +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("regdump @ %p\n", g_dev); + g_dev->print_registers(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("missing command: try 'start', 'info', 'test', 'reset' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -1307,5 +1340,11 @@ l3gd20_main(int argc, char *argv[]) if (!strcmp(verb, "info")) l3gd20::info(); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) + l3gd20::regdump(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } From 1992f0bf54ee7b4438f9ea48ca2a4e9bfff0a75f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Dec 2014 16:07:47 +1100 Subject: [PATCH 021/293] l3gd20: use the I2C disable bit on l3gd20H this seems to prevent a mpu6000 reset from causing an issue on the l3gd20H --- src/drivers/l3gd20/l3gd20.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 63974a7df046..3a0c05c9e20f 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -142,6 +142,7 @@ static const int ERROR = -1; #define ADDR_INT1_TSH_ZH 0x36 #define ADDR_INT1_TSH_ZL 0x37 #define ADDR_INT1_DURATION 0x38 +#define ADDR_LOW_ODR 0x39 /* Internal configuration values */ @@ -244,7 +245,7 @@ class L3GD20 : public device::SPI // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define L3GD20_NUM_CHECKED_REGISTERS 7 +#define L3GD20_NUM_CHECKED_REGISTERS 8 static const uint8_t _checked_registers[L3GD20_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -375,7 +376,8 @@ const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_ ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, - ADDR_FIFO_CTRL_REG }; + ADDR_FIFO_CTRL_REG, + ADDR_LOW_ODR }; L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), @@ -844,6 +846,11 @@ L3GD20::disable_i2c(void) uint8_t a = read_reg(0x05); write_reg(0x05, (0x20 | a)); if (read_reg(0x05) == (a | 0x20)) { + // this sets the I2C_DIS bit on the + // L3GD20H. The l3gd20 datasheet doesn't + // mention this register, but it does seem to + // accept it. + write_checked_reg(ADDR_LOW_ODR, 0x08); return; } } From 140784092d838cca0b22ce5083fd079789b812f0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 20 Dec 2014 17:35:31 +0900 Subject: [PATCH 022/293] NuttxConfig: increase I2C timeout to 10ms --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index d31648694b1c..e0e51acfba79 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -323,7 +323,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=1 +CONFIG_STM32_I2CTIMEOMS=10 # CONFIG_STM32_I2C_DUTY16_9 is not set # From 42731ccf8964a490a70f62fb486ce7db533a7409 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 30 Dec 2014 11:13:05 +0900 Subject: [PATCH 023/293] i2c: const get_address --- src/drivers/device/i2c.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 705b398b0537..206e71ddcc7b 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -58,7 +58,7 @@ class __EXPORT I2C : public CDev /** * Get the address */ - int16_t get_address() { return _address; } + int16_t get_address() const { return _address; } protected: /** From 8ba3569fcad7a17b4a21aa7c5ded7dad446587f6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 25 Nov 2014 15:03:44 +0900 Subject: [PATCH 024/293] batt_smbus: driver for smart battery --- src/drivers/batt_smbus/batt_smbus.cpp | 438 ++++++++++++++++++++++++++ src/drivers/batt_smbus/module.mk | 8 + src/drivers/drv_batt_smbus.h | 50 +++ 3 files changed, 496 insertions(+) create mode 100644 src/drivers/batt_smbus/batt_smbus.cpp create mode 100644 src/drivers/batt_smbus/module.mk create mode 100644 src/drivers/drv_batt_smbus.h diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp new file mode 100644 index 000000000000..89abcf83858e --- /dev/null +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -0,0 +1,438 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Randy Mackay + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file batt_smbus.cpp + * + * Driver for a battery monitor connected via SMBus (I2C). + * + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION +#define BATT_SMBUS_ADDR 0x0B /* I2C address */ +#define BATT_SMBUS_TEMP 0x08 /* temperature register */ +#define BATT_SMBUS_VOLTAGE 0x09 /* voltage register */ +#define BATT_SMBUS_DESIGN_CAPACITY 0x18 /* design capacity register */ +#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 /* design voltage register */ +#define BATT_SMBUS_SERIALNUM 0x1c /* serial number register */ +#define BATT_SMBUS_MANUFACTURE_INFO 0x25 /* cell voltage register */ +#define BATT_SMBUS_CURRENT 0x2a /* current register */ +#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) /* time in microseconds, measure at 10hz */ + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class BATT_SMBUS : public device::I2C +{ +public: + BATT_SMBUS(int bus = PX4_I2C_BUS_EXPANSION, uint16_t batt_smbus_addr = BATT_SMBUS_ADDR); + virtual ~BATT_SMBUS(); + + virtual int init(); + virtual int test(); + +protected: + virtual int probe(); // check if the device can be contacted + +private: + + // start periodic reads from the battery + void start(); + + // stop periodic reads from the battery + void stop(); + + // static function that is called by worker queue + static void cycle_trampoline(void *arg); + + // perform a read from the battery + void cycle(); + + // read_reg - read a word from specified register + int read_reg(uint8_t reg, uint16_t &val); + + // read_block - returns number of characters read if successful, zero if unsuccessful + uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero); + + // internal variables + work_s _work; // work queue for scheduling reads + RingBuffer *_reports; // buffer of recorded voltages, currents + struct battery_status_s _last_report; // last published report, used for test() + orb_advert_t _batt_topic; + orb_id_t _batt_orb_id; +}; + +/* for now, we only support one BATT_SMBUS */ +namespace +{ +BATT_SMBUS *g_batt_smbus; +} + +void batt_smbus_usage(); + +extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); + +// constructor +BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : + I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), + _work{}, + _reports(nullptr), + _batt_topic(-1), + _batt_orb_id(nullptr) +{ + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +// destructor +BATT_SMBUS::~BATT_SMBUS() +{ + /* make sure we are truly inactive */ + stop(); + + if (_reports != nullptr) { + delete _reports; + } +} + +int +BATT_SMBUS::init() +{ + int ret = ENOTTY; + + // attempt to initialise I2C bus + ret = I2C::init(); + + if (ret != OK) { + errx(1,"failed to init I2C"); + return ret; + } else { + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(struct battery_status_s)); + if (_reports == nullptr) { + ret = ENOTTY; + } else { + // start work queue + start(); + } + } + + // init orb id + _batt_orb_id = ORB_ID(battery_status); + + return ret; +} + +int +BATT_SMBUS::test() +{ + int sub = orb_subscribe(ORB_ID(battery_status)); + bool updated = false; + struct battery_status_s status; + uint64_t start_time = hrt_absolute_time(); + + // loop for 5 seconds + while ((hrt_absolute_time() - start_time) < 5000000) { + + // display new info that has arrived from the orb + orb_check(sub, &updated); + if (updated) { + if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { + warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a); + } + } + + // sleep for 0.05 seconds + usleep(50000); + } + + return OK; +} + +int +BATT_SMBUS::probe() +{ + // always return OK to ensure device starts + return OK; +} + +// start periodic reads from the battery +void +BATT_SMBUS::start() +{ + /* reset the report ring and state machine */ + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1); +} + +// stop periodic reads from the battery +void +BATT_SMBUS::stop() +{ + work_cancel(HPWORK, &_work); +} + +// static function that is called by worker queue +void +BATT_SMBUS::cycle_trampoline(void *arg) +{ + BATT_SMBUS *dev = (BATT_SMBUS *)arg; + + dev->cycle(); +} + +// perform a read from the battery +void +BATT_SMBUS::cycle() +{ + // read data from sensor + struct battery_status_s new_report; + + // set time of reading + new_report.timestamp = hrt_absolute_time(); + + // read voltage + uint16_t tmp; + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { + // initialise new_report + memset(&new_report, 0, sizeof(new_report)); + + // convert millivolts to volts + new_report.voltage_v = ((float)tmp) / 1000.0f; + + // To-Do: read current as block from BATT_SMBUS_CURRENT register + + // publish to orb + if (_batt_topic != -1) { + orb_publish(_batt_orb_id, _batt_topic, &new_report); + } else { + _batt_topic = orb_advertise(_batt_orb_id, &new_report); + if (_batt_topic < 0) { + errx(1, "ADVERT FAIL"); + } + } + + // copy report for test() + _last_report = new_report; + + /* post a report to the ring */ + _reports->force(&new_report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); +} + +int +BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) +{ + uint8_t buff[2]; + + // short sleep to improve reliability in cases of two consecutive reads + usleep(1); + + // read from register + int ret = transfer(®, 1, buff, 2); + if (ret == OK) { + val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + } + + // return success or failure + return ret; +} + +// read_block - returns number of characters read if successful, zero if unsuccessful +uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) +{ + uint8_t buff[max_len+1]; // buffer to hold results + + usleep(1); + + // read bytes + int ret = transfer(®, 1,buff, max_len+1); + + // return zero on failure + if (ret != OK) { + return 0; + } + + // get length + uint8_t bufflen = buff[0]; + + // sanity check length returned by smbus + if (bufflen == 0 || bufflen > max_len) { + return 0; + } + + // copy data + memcpy(data, &buff[1], bufflen); + + // optionally add zero to end + if (append_zero) { + data[bufflen] = '\0'; + } + + // return success + return bufflen; +} + +///////////////////////// shell functions /////////////////////// + +void +batt_smbus_usage() +{ + warnx("missing command: try 'start', 'test', 'stop'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", BATT_SMBUS_I2C_BUS); + warnx(" -a addr (0x%x)", BATT_SMBUS_ADDR); +} + +int +batt_smbus_main(int argc, char *argv[]) +{ + int i2cdevice = BATT_SMBUS_I2C_BUS; + int batt_smbusadr = BATT_SMBUS_ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + batt_smbusadr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + batt_smbus_usage(); + exit(0); + } + } + + if (optind >= argc) { + batt_smbus_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + if (!strcmp(verb, "start")) { + if (g_batt_smbus != nullptr) { + errx(1, "already started"); + } else { + // create new global object + g_batt_smbus = new BATT_SMBUS(i2cdevice, batt_smbusadr); + + if (g_batt_smbus == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_batt_smbus->init()) { + delete g_batt_smbus; + g_batt_smbus = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + /* need the driver past this point */ + if (g_batt_smbus == nullptr) { + warnx("not started"); + batt_smbus_usage(); + exit(1); + } + + if (!strcmp(verb, "test")) { + g_batt_smbus->test(); + exit(0); + } + + if (!strcmp(verb, "stop")) { + delete g_batt_smbus; + g_batt_smbus = nullptr; + exit(0); + } + + batt_smbus_usage(); + exit(0); +} diff --git a/src/drivers/batt_smbus/module.mk b/src/drivers/batt_smbus/module.mk new file mode 100644 index 000000000000..b32ea6e55f72 --- /dev/null +++ b/src/drivers/batt_smbus/module.mk @@ -0,0 +1,8 @@ +# +# driver for SMBus smart batteries +# + +MODULE_COMMAND = batt_smbus +SRCS = batt_smbus.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h new file mode 100644 index 000000000000..6bba5fe167cd --- /dev/null +++ b/src/drivers/drv_batt_smbus.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_batt_smbus.h + * + * SMBus battery monitor device API + */ + +#pragma once + +#include +#include +#include "drv_orb_dev.h" + +/* device path */ +#define BATT_SMBUS_DEVICE_PATH "/dev/batt_smbus" + +/* ObjDev tag for battery data */ +ORB_DECLARE(battery_status); From 9c6c17d99a53166d440600eef8b1103f2c6effff Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 9 Dec 2014 19:29:55 +0900 Subject: [PATCH 025/293] batt_smbus: remove sleep before I2C transfer --- src/drivers/batt_smbus/batt_smbus.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 89abcf83858e..494187ac02d3 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -303,9 +303,6 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) { uint8_t buff[2]; - // short sleep to improve reliability in cases of two consecutive reads - usleep(1); - // read from register int ret = transfer(®, 1, buff, 2); if (ret == OK) { From 920be507ce55f42d54b18fbc9183fae23289f9df Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Dec 2014 16:42:47 +0900 Subject: [PATCH 026/293] batt_smbus: remove redundant ORB_DECLARE --- src/drivers/drv_batt_smbus.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h index 6bba5fe167cd..ca130c84eeb0 100644 --- a/src/drivers/drv_batt_smbus.h +++ b/src/drivers/drv_batt_smbus.h @@ -45,6 +45,3 @@ /* device path */ #define BATT_SMBUS_DEVICE_PATH "/dev/batt_smbus" - -/* ObjDev tag for battery data */ -ORB_DECLARE(battery_status); From 5e2604a9927dffbae581a316a589953a3996b2cb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 29 Dec 2014 21:09:32 +0900 Subject: [PATCH 027/293] batt_smbus: minor format fix --- src/drivers/batt_smbus/batt_smbus.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 494187ac02d3..882b38f7ec8d 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -304,7 +304,7 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) uint8_t buff[2]; // read from register - int ret = transfer(®, 1, buff, 2); + int ret = transfer(®, 1, buff, 2); if (ret == OK) { val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; } From d5e4ac18b22b9068e3fd989f5c8d4b7115298027 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Dec 2014 16:42:29 +0900 Subject: [PATCH 028/293] batt_smbus: add get_PEC --- src/drivers/batt_smbus/batt_smbus.cpp | 84 ++++++++++++++++++++++++--- 1 file changed, 77 insertions(+), 7 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 882b38f7ec8d..4d4a84049df5 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -84,10 +84,13 @@ #define BATT_SMBUS_DESIGN_CAPACITY 0x18 /* design capacity register */ #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 /* design voltage register */ #define BATT_SMBUS_SERIALNUM 0x1c /* serial number register */ +#define BATT_SMBUS_MANUFACTURE_NAME 0x20 /* manufacturer name */ #define BATT_SMBUS_MANUFACTURE_INFO 0x25 /* cell voltage register */ #define BATT_SMBUS_CURRENT 0x2a /* current register */ #define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) /* time in microseconds, measure at 10hz */ +#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 /* Polynomial for calculating PEC */ + #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif @@ -124,6 +127,10 @@ class BATT_SMBUS : public device::I2C // read_block - returns number of characters read if successful, zero if unsuccessful uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero); + // get_PEC - calculate PEC for a read or write from the battery + // buff is the data that was read or will be written + uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; + // internal variables work_s _work; // work queue for scheduling reads RingBuffer *_reports; // buffer of recorded voltages, currents @@ -272,7 +279,12 @@ BATT_SMBUS::cycle() // convert millivolts to volts new_report.voltage_v = ((float)tmp) / 1000.0f; - // To-Do: read current as block from BATT_SMBUS_CURRENT register + // read current + usleep(1); + uint8_t buff[4]; + if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { + new_report.current_a = (float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f; + } // publish to orb if (_batt_topic != -1) { @@ -301,12 +313,20 @@ BATT_SMBUS::cycle() int BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) { - uint8_t buff[2]; + uint8_t buff[3]; // 2 bytes of data + PEC // read from register - int ret = transfer(®, 1, buff, 2); + int ret = transfer(®, 1, buff, 3); if (ret == OK) { - val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + // check PEC + uint8_t pec = get_PEC(reg, true, buff, 2); + if (pec == buff[2]) { + val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + warnx("PEC ok: %x",(int)pec); + } else { + ret = ENOTTY; + warnx("PEC:%x vs MyPec:%x",(int)buff[2],(int)pec); + } } // return success or failure @@ -316,12 +336,12 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) // read_block - returns number of characters read if successful, zero if unsuccessful uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) { - uint8_t buff[max_len+1]; // buffer to hold results + uint8_t buff[max_len+2]; // buffer to hold results usleep(1); - // read bytes - int ret = transfer(®, 1,buff, max_len+1); + // read bytes including PEC + int ret = transfer(®, 1, buff, max_len+2); // return zero on failure if (ret != OK) { @@ -336,6 +356,16 @@ uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool return 0; } + // check PEC + uint8_t pec = get_PEC(reg, true, buff, bufflen+1); + if (pec != buff[bufflen+1]) { + // debug + warnx("CurrPEC:%x vs MyPec:%x",(int)buff[bufflen+1],(int)pec); + return 0; + } else { + warnx("CurPEC ok: %x",(int)pec); + } + // copy data memcpy(data, &buff[1], bufflen); @@ -348,6 +378,46 @@ uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool return bufflen; } +// get_PEC - calculate PEC for a read or write from the battery +// buff is the data that was read or will be written +uint8_t BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const +{ + // exit immediately if no data + if (len <= 0) { + return 0; + } + + // prepare temp buffer for calcing crc + uint8_t tmp_buff[len+3]; + tmp_buff[0] = (uint8_t)get_address() << 1; + tmp_buff[1] = cmd; + tmp_buff[2] = tmp_buff[0] | (uint8_t)reading; + memcpy(&tmp_buff[3],buff,len); + + // initialise crc to zero + uint8_t crc = 0; + uint8_t shift_reg = 0; + bool do_invert; + + // for each byte in the stream + for (uint8_t i=0; i Date: Tue, 30 Dec 2014 12:14:54 +0900 Subject: [PATCH 029/293] batt_smbus: add search --- src/drivers/batt_smbus/batt_smbus.cpp | 41 +++++++++++++++++++++++++-- 1 file changed, 38 insertions(+), 3 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 4d4a84049df5..dd83dacafca3 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -77,6 +77,9 @@ #include #include +#define BATT_SMBUS_ADDR_MIN 0x08 /* lowest possible address */ +#define BATT_SMBUS_ADDR_MAX 0x7F /* highest possible address */ + #define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION #define BATT_SMBUS_ADDR 0x0B /* I2C address */ #define BATT_SMBUS_TEMP 0x08 /* temperature register */ @@ -103,6 +106,7 @@ class BATT_SMBUS : public device::I2C virtual int init(); virtual int test(); + int search(); /* search all possible slave addresses for a smart battery */ protected: virtual int probe(); // check if the device can be contacted @@ -226,6 +230,34 @@ BATT_SMBUS::test() return OK; } +/* search all possible slave addresses for a smart battery */ +int +BATT_SMBUS::search() +{ + bool found_slave = false; + uint16_t tmp; + + // search through all valid SMBus addresses + for (uint8_t i=BATT_SMBUS_ADDR_MIN; i<=BATT_SMBUS_ADDR_MAX; i++) { + set_address(i); + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { + warnx("battery found at 0x%x",(int)i); + found_slave = true; + } + // short sleep + usleep(1); + } + + // display completion message + if (found_slave) { + warnx("Done."); + } else { + warnx("No smart batteries found."); + } + + return OK; +} + int BATT_SMBUS::probe() { @@ -322,10 +354,8 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) uint8_t pec = get_PEC(reg, true, buff, 2); if (pec == buff[2]) { val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; - warnx("PEC ok: %x",(int)pec); } else { ret = ENOTTY; - warnx("PEC:%x vs MyPec:%x",(int)buff[2],(int)pec); } } @@ -423,7 +453,7 @@ uint8_t BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uin void batt_smbus_usage() { - warnx("missing command: try 'start', 'test', 'stop'"); + warnx("missing command: try 'start', 'test', 'stop', 'search'"); warnx("options:"); warnx(" -b i2cbus (%d)", BATT_SMBUS_I2C_BUS); warnx(" -a addr (0x%x)", BATT_SMBUS_ADDR); @@ -500,6 +530,11 @@ batt_smbus_main(int argc, char *argv[]) exit(0); } + if (!strcmp(verb, "search")) { + g_batt_smbus->search(); + exit(0); + } + batt_smbus_usage(); exit(0); } From d7c11da65c0b06bb1deaed5ce10832636460a661 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:42:45 +1100 Subject: [PATCH 030/293] l3gd20: added testerror command useful for testing error handling --- src/drivers/l3gd20/l3gd20.cpp | 35 +++++++++++++++++++++++++++++++++-- 1 file changed, 33 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 3a0c05c9e20f..e5ace48c94a2 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -204,6 +204,9 @@ class L3GD20 : public device::SPI // print register dump void print_registers(); + // trigger an error + void test_error(); + protected: virtual int probe(); @@ -1098,6 +1101,13 @@ L3GD20::print_registers() printf("\n"); } +void +L3GD20::test_error() +{ + // trigger a deliberate error + write_reg(ADDR_CTRL_REG3, 0); +} + int L3GD20::self_test() { @@ -1287,10 +1297,25 @@ regdump(void) exit(0); } +/** + * trigger an error + */ +void +test_error(void) +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("regdump @ %p\n", g_dev); + g_dev->test_error(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset' or 'regdump'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -1353,5 +1378,11 @@ l3gd20_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) l3gd20::regdump(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + /* + * trigger an error + */ + if (!strcmp(verb, "testerror")) + l3gd20::test_error(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } From 12517b7c191c8745574d700cac8ff50e169855e5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:43:13 +1100 Subject: [PATCH 031/293] l3gd20: removed printf in interrupt context this is not safe --- src/drivers/l3gd20/l3gd20.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index e5ace48c94a2..71a54c0f930e 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -924,14 +924,6 @@ L3GD20::check_registers(void) if (_checked_next != 0) { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); } -#if 1 - if (_register_wait == 0) { - ::printf("L3GD20: %02x:%02x should be %02x\n", - (unsigned)_checked_registers[_checked_next], - (unsigned)v, - (unsigned)_checked_values[_checked_next]); - } -#endif _register_wait = 20; } _checked_next = (_checked_next+1) % L3GD20_NUM_CHECKED_REGISTERS; From d4115983071797110c7d3e4745933a29658101a0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:43:36 +1100 Subject: [PATCH 032/293] l3gd20: check DRDY after check_registers() is called this allows us to recover from an error that disables data ready --- src/drivers/l3gd20/l3gd20.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 71a54c0f930e..5c9235017032 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -932,17 +932,6 @@ L3GD20::check_registers(void) void L3GD20::measure() { -#if L3GD20_USE_DRDY - // if the gyro doesn't have any data ready then re-schedule - // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value - if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { - perf_count(_reschedules); - hrt_call_delay(&_call, 100); - return; - } -#endif - /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -962,6 +951,18 @@ L3GD20::measure() check_registers(); +#if L3GD20_USE_DRDY + // if the gyro doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { + perf_count(_reschedules); + hrt_call_delay(&_call, 100); + perf_end(_sample_perf); + return; + } +#endif + /* fetch data from the sensor */ memset(&raw_report, 0, sizeof(raw_report)); raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; From bfed6b2a71a6c617480feb803f4bdcc9445380d7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:44:12 +1100 Subject: [PATCH 033/293] l3gd20: fixed reporting of error count --- src/drivers/l3gd20/l3gd20.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 5c9235017032..4e2f198b8ae7 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -993,7 +993,7 @@ L3GD20::measure() * 74 from all measurements centers them around zero. */ report.timestamp = hrt_absolute_time(); - report.error_count = 0; // not recorded + report.error_count = perf_event_count(_bad_registers); switch (_orientation) { From cd0e66873ac1734ec30020e773b402721bd49ed8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:44:50 +1100 Subject: [PATCH 034/293] l3m303d: added testerror command useful for testing error handling --- src/drivers/lsm303d/lsm303d.cpp | 36 +++++++++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index dbd5c1f4c3a9..a062b2828673 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -237,6 +237,11 @@ class LSM303D : public device::SPI */ void print_registers(); + /** + * deliberately trigger an error + */ + void test_error(); + protected: virtual int probe(); @@ -1690,6 +1695,13 @@ LSM303D::print_registers() } } +void +LSM303D::test_error() +{ + // trigger an error + write_reg(ADDR_CTRL_REG3, 0); +} + LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), _parent(parent), @@ -1969,10 +1981,24 @@ regdump() exit(0); } +/** + * trigger an error + */ +void +test_error() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + g_dev->test_error(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2035,5 +2061,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) lsm303d::regdump(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', or 'regdump'"); + /* + * trigger an error + */ + if (!strcmp(verb, "testerror")) + lsm303d::test_error(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } From 3329af71ca33e73c8d3cf4e74c37f125cfa5a022 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:45:25 +1100 Subject: [PATCH 035/293] lsm303d: added two more checked registers these are key for DRDY behaviour --- src/drivers/lsm303d/lsm303d.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a062b2828673..ace228920087 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -297,7 +297,7 @@ class LSM303D : public device::SPI // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define LSM303D_NUM_CHECKED_REGISTERS 6 +#define LSM303D_NUM_CHECKED_REGISTERS 8 static const uint8_t _checked_registers[LSM303D_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -474,6 +474,8 @@ class LSM303D : public device::SPI const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, ADDR_CTRL_REG1, ADDR_CTRL_REG2, + ADDR_CTRL_REG3, + ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, ADDR_CTRL_REG7 }; @@ -708,8 +710,8 @@ LSM303D::reset() /* enable mag */ write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 - write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 + write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 + write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); From 7b3e24d65c1bfa828e64c73cedddcb4a29c7c257 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:45:51 +1100 Subject: [PATCH 036/293] lsm303d: removed unsafe printf in interrupt context --- src/drivers/lsm303d/lsm303d.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index ace228920087..58dcdf410af7 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1429,14 +1429,6 @@ LSM303D::check_registers(void) if (_checked_next != 0) { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); } -#if 1 - if (_register_wait == 0) { - ::printf("LSM303D: %02x:%02x should be %02x\n", - (unsigned)_checked_registers[_checked_next], - (unsigned)v, - (unsigned)_checked_values[_checked_next]); - } -#endif _register_wait = 20; } _checked_next = (_checked_next+1) % LSM303D_NUM_CHECKED_REGISTERS; From 94a6384b6530ecae9006978a902a98b7c5b79b3d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:46:13 +1100 Subject: [PATCH 037/293] lsm303d: check DRDY after check_registers() this allows recovery from a state where DRDY is not set --- src/drivers/lsm303d/lsm303d.cpp | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 58dcdf410af7..3e5ff1f7cea1 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1437,19 +1437,6 @@ LSM303D::check_registers(void) void LSM303D::measure() { - // if the accel doesn't have any data ready then re-schedule - // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value. - // Note that DRDY is not available when the lsm303d is - // connected on the external bus -#ifdef GPIO_EXTI_ACCEL_DRDY - if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { - perf_count(_accel_reschedules); - hrt_call_delay(&_accel_call, 100); - return; - } -#endif - /* status register and data as read back from the device */ #pragma pack(push, 1) @@ -1469,6 +1456,20 @@ LSM303D::measure() check_registers(); + // if the accel doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value. + // Note that DRDY is not available when the lsm303d is + // connected on the external bus +#ifdef GPIO_EXTI_ACCEL_DRDY + if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { + perf_count(_accel_reschedules); + hrt_call_delay(&_accel_call, 100); + perf_end(_accel_sample_perf); + return; + } +#endif + if (_register_wait != 0) { // we are waiting for some good transfers before using // the sensor again. From 95364f6a55e973b29ede8c6e675ba54b3967b54b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:46:40 +1100 Subject: [PATCH 038/293] lsm303d: show all perf counters in "info" --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 3e5ff1f7cea1..4f4e5f39c459 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1618,7 +1618,9 @@ LSM303D::print_info() printf("accel reads: %u\n", _accel_read); printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); + perf_print_counter(_mag_sample_perf); perf_print_counter(_bad_registers); + perf_print_counter(_accel_reschedules); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); ::printf("checked_next: %u\n", _checked_next); From 71df17119802a398782e8cd726e6cca1c0aa62a2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:47:07 +1100 Subject: [PATCH 039/293] mpu6000: removed unsafe printf in interrupt context instead delay 3ms between register writes. This seems to give a quite high probability of correctly resetting the sensor, and does still reliably detect the sensor going bad, which is the most important thing in this code --- src/drivers/mpu6000/mpu6000.cpp | 42 ++++----------------------------- 1 file changed, 4 insertions(+), 38 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index e7e88bb82b11..6cac28a7dbc8 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -254,7 +254,6 @@ class MPU6000 : public device::SPI uint8_t _register_wait; uint64_t _reset_wait; - uint64_t _printf_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -494,7 +493,6 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), _reset_wait(0), - _printf_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -1539,43 +1537,11 @@ MPU6000::check_registers(void) _checked_next = 0; } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); - _reset_wait = hrt_absolute_time() + 1000; - if (_checked_next == 1 && hrt_absolute_time() > _printf_wait) { - /* - rather bizarrely this printf() seems - to be critical for successfully - resetting the sensor. If you take - this printf out then all the - registers do get successfully reset, - but the sensor ends up with a large - fixed offset on the - accelerometers. Try a up_udelay() - of various sizes instead of a - printf() doesn't work. That makes no - sense at all, and investigating why - this is would be worthwhile. - - The rate limit on the printf to 5Hz - prevents this from causing enough - delays in interrupt context to cause - the vehicle to not be able to fly - correctly. - */ - _printf_wait = hrt_absolute_time() + 200*1000UL; - ::printf("Setting %u %02x to %02x\n", - (unsigned)_checked_next, - (unsigned)_checked_registers[_checked_next], - (unsigned)_checked_values[_checked_next]); - } + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; } -#if 0 - if (_register_wait == 0) { - ::printf("MPU6000: %02x:%02x should be %02x\n", - (unsigned)_checked_registers[_checked_next], - (unsigned)v, - (unsigned)_checked_values[_checked_next]); - } -#endif _register_wait = 20; } _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; From fe77c5ff3d5250ca1748a789ec4881e2117f4cf1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 10:03:34 +1100 Subject: [PATCH 040/293] l3gd20: fixed build warning --- src/drivers/l3gd20/l3gd20.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4e2f198b8ae7..08bc1fead857 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1137,6 +1137,7 @@ void test(); void reset(); void info(); void regdump(); +void test_error(); /** * Start the driver. From 2a076d75023bbe7a414fa3c5b9ac9a18a617454f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 10:03:42 +1100 Subject: [PATCH 041/293] lsm303d: fixed build warning --- src/drivers/lsm303d/lsm303d.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4f4e5f39c459..00484db79781 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1773,6 +1773,7 @@ void reset(); void info(); void regdump(); void usage(); +void test_error(); /** * Start the driver. From cf9189665d8d84608fc7482070bf3a47a9fa6c0a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 10:59:42 +1100 Subject: [PATCH 042/293] ll40ls: fixed exit code on external sensor startup failure --- src/drivers/ll40ls/ll40ls.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 6793acd810a6..39e1a340ebf0 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -744,6 +744,9 @@ start(int bus) if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { delete g_dev_ext; g_dev_ext = nullptr; + if (bus == PX4_I2C_BUS_EXPANSION) { + goto fail; + } } } From ce6026583a59e981d1fd10172c1dd69f94526e87 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Jan 2015 16:45:08 +1100 Subject: [PATCH 043/293] lsm303d: detect large fixed values and report an error we have logs where the lsm303d gets large fixed values for long periods. This will detect that error case and raise error_count so the higher level sensor integration code can choose another sensor --- src/drivers/lsm303d/lsm303d.cpp | 49 +++++++++++++++++++++++++++++---- 1 file changed, 44 insertions(+), 5 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 00484db79781..57754e4c0d8f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -285,6 +285,7 @@ class LSM303D : public device::SPI perf_counter_t _mag_sample_perf; perf_counter_t _accel_reschedules; perf_counter_t _bad_registers; + perf_counter_t _bad_values; uint8_t _register_wait; @@ -294,6 +295,10 @@ class LSM303D : public device::SPI enum Rotation _rotation; + // values used to + float _last_accel[3]; + uint8_t _constant_accel_count; + // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset @@ -542,11 +547,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), _bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")), + _bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_values")), _register_wait(0), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), + _constant_accel_count(0), _checked_next(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; @@ -590,6 +597,7 @@ LSM303D::~LSM303D() perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); perf_free(_bad_registers); + perf_free(_bad_values); perf_free(_accel_reschedules); } @@ -1501,11 +1509,11 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); - // report the error count as the sum of the number of bad bad - // register reads. This allows the higher level code to decide - // if it should use this sensor based on whether it has had - // failures - accel_report.error_count = perf_event_count(_bad_registers); + // report the error count as the sum of the number of bad + // register reads and bad values. This allows the higher level + // code to decide if it should use this sensor based on + // whether it has had failures + accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); accel_report.x_raw = raw_accel_report.x; accel_report.y_raw = raw_accel_report.y; @@ -1515,6 +1523,36 @@ LSM303D::measure() float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + /* + we have logs where the accelerometers get stuck at a fixed + large value. We want to detect this and mark the sensor as + being faulty + */ + if (fabsf(_last_accel[0] - x_in_new) < 0.001f && + fabsf(_last_accel[1] - y_in_new) < 0.001f && + fabsf(_last_accel[2] - z_in_new) < 0.001f && + fabsf(x_in_new) > 20 && + fabsf(y_in_new) > 20 && + fabsf(z_in_new) > 20) { + _constant_accel_count += 1; + } else { + _constant_accel_count = 0; + } + if (_constant_accel_count > 100) { + // we've had 100 constant accel readings with large + // values. The sensor is almost certainly dead. We + // will raise the error_count so that the top level + // flight code will know to avoid this sensor, but + // we'll still give the data so that it can be logged + // and viewed + perf_count(_bad_values); + _constant_accel_count = 0; + } + + _last_accel[0] = x_in_new; + _last_accel[1] = y_in_new; + _last_accel[2] = z_in_new; + accel_report.x = _accel_filter_x.apply(x_in_new); accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); @@ -1620,6 +1658,7 @@ LSM303D::print_info() perf_print_counter(_accel_sample_perf); perf_print_counter(_mag_sample_perf); perf_print_counter(_bad_registers); + perf_print_counter(_bad_values); perf_print_counter(_accel_reschedules); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); From 97655fdc95ee2f4e9013a889f753c2912328b1f6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 6 Jan 2015 09:36:49 +1100 Subject: [PATCH 044/293] uavcan: use UAVCAN variable for uavcan directory --- src/modules/uavcan/module.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index e5d30f6c47a3..f92bc754f032 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp \ # # libuavcan # -include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk +include $(UAVCAN_DIR)/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile @@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV # # libuavcan drivers for STM32 # -include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk +include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 From 061d414902e31853a0d1d6cc7bce92b24778dabb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Jan 2015 17:23:32 +1100 Subject: [PATCH 045/293] hmc5883: fixed handling of 3 bus options use a table of possible bus options. This prevents us starting two drivers on the same bus --- src/drivers/hmc5883/hmc5883.cpp | 309 +++++++++++++------------------- src/drivers/hmc5883/hmc5883.h | 1 + 2 files changed, 123 insertions(+), 187 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fe70bd37fece..2a10b006322a 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -80,9 +80,6 @@ * HMC5883 internal constants and data structures. */ -#define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int" -#define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext" - /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ @@ -114,9 +111,10 @@ #define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ enum HMC5883_BUS { - HMC5883_BUS_ALL, - HMC5883_BUS_INTERNAL, - HMC5883_BUS_EXTERNAL + HMC5883_BUS_ALL = 0, + HMC5883_BUS_I2C_INTERNAL, + HMC5883_BUS_I2C_EXTERNAL, + HMC5883_BUS_SPI }; /* oddly, ERROR is not defined for c++ */ @@ -1297,169 +1295,129 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev_int = nullptr; -HMC5883 *g_dev_ext = nullptr; - -void start(int bus, enum Rotation rotation); -void test(int bus); -void reset(int bus); -int info(int bus); -int calibrate(int bus); -const char* get_path(int bus); +/* + list of supported bus configurations + */ +struct hmc5883_bus_option { + enum HMC5883_BUS busid; + const char *devpath; + HMC5883_constructor interface_constructor; + uint8_t busnum; + HMC5883 *dev; +} bus_options[] = { + { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#ifdef PX4_I2C_BUS_ONBOARD + { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_SPIDEV_HMC + { HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +void start(enum HMC5883_BUS busid, enum Rotation rotation); +bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation); +struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid); +void test(enum HMC5883_BUS busid); +void reset(enum HMC5883_BUS busid); +int info(enum HMC5883_BUS busid); +int calibrate(enum HMC5883_BUS busid); void usage(); /** - * Start the driver. - * - * This function call only returns once the driver - * is either successfully up and running or failed to start. + * start driver for a specific bus option */ -void -start(int external_bus, enum Rotation rotation) +bool +start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) { - int fd; - - /* create the driver, attempt expansion bus first */ - if (g_dev_ext != nullptr) { - warnx("already started external"); - } else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) { - - device::Device *interface = nullptr; - - /* create the driver, only attempt I2C for the external bus */ - if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { - interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION); - - if (interface->init() != OK) { - delete interface; - interface = nullptr; - warnx("no device on I2C bus #%u", PX4_I2C_BUS_EXPANSION); - } - } - -#ifdef PX4_I2C_BUS_ONBOARD - if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { - interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); - - if (interface->init() != OK) { - delete interface; - interface = nullptr; - warnx("no device on I2C bus #%u", PX4_I2C_BUS_ONBOARD); - } - } -#endif - - /* interface will be null if init failed */ - if (interface != nullptr) { - - g_dev_ext = new HMC5883(interface, HMC5883L_DEVICE_PATH_EXT, rotation); - if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { - delete g_dev_ext; - g_dev_ext = nullptr; - } - - } + if (bus.dev != nullptr) + errx(1,"bus option already started"); + + device::Device *interface = bus.interface_constructor(bus.busnum); + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u", (unsigned)bus.busid); + return false; + } + bus.dev = new HMC5883(interface, bus.devpath, rotation); + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; } + int fd = open(bus.devpath, O_RDONLY); + if (fd < 0) + return false; - /* if this failed, attempt onboard sensor */ - if (g_dev_int != nullptr) { - warnx("already started internal"); - } else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) { - - device::Device *interface = nullptr; - - /* create the driver, try SPI first, fall back to I2C if unsuccessful */ -#ifdef PX4_SPIDEV_HMC - if (HMC5883_SPI_interface != nullptr) { - interface = HMC5883_SPI_interface(PX4_SPI_BUS_SENSORS); - } -#endif - -#ifdef PX4_I2C_BUS_ONBOARD - /* this device is already connected as external if present above */ - if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { - interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); - } -#endif - if (interface == nullptr) { - warnx("no internal bus scanned"); - goto fail; - } + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + close(fd); + errx(1,"Failed to setup poll rate"); + } - if (interface->init() != OK) { - delete interface; - warnx("no device on internal bus"); - } else { + return true; +} - g_dev_int = new HMC5883(interface, HMC5883L_DEVICE_PATH_INT, rotation); - if (g_dev_int != nullptr && OK != g_dev_int->init()) { - /* tear down the failing onboard instance */ - delete g_dev_int; - g_dev_int = nullptr; +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +void +start(enum HMC5883_BUS busid, enum Rotation rotation) +{ + uint8_t i; + bool started = false; - if (external_bus == HMC5883_BUS_INTERNAL) { - goto fail; - } - } - if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) { - goto fail; - } + for (i=0; iprint_info(); - ret = 0; - } - - return ret; -} - -const char* -get_path(int bus) -{ - return ((bus == HMC5883_BUS_INTERNAL) ? HMC5883L_DEVICE_PATH_INT : HMC5883L_DEVICE_PATH_EXT); + warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath); + bus.dev->print_info(); + exit(0); } void @@ -1650,22 +1595,25 @@ int hmc5883_main(int argc, char *argv[]) { int ch; - int bus = HMC5883_BUS_ALL; + enum HMC5883_BUS busid = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; - while ((ch = getopt(argc, argv, "XIR:C")) != EOF) { + while ((ch = getopt(argc, argv, "XISR:C")) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(optarg); break; #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) case 'I': - bus = HMC5883_BUS_INTERNAL; + busid = HMC5883_BUS_I2C_INTERNAL; break; #endif case 'X': - bus = HMC5883_BUS_EXTERNAL; + busid = HMC5883_BUS_I2C_EXTERNAL; + break; + case 'S': + busid = HMC5883_BUS_SPI; break; case 'C': calibrate = true; @@ -1682,9 +1630,9 @@ hmc5883_main(int argc, char *argv[]) * Start/load the driver. */ if (!strcmp(verb, "start")) { - hmc5883::start(bus, rotation); + hmc5883::start(busid, rotation); if (calibrate) { - if (hmc5883::calibrate(bus) == 0) { + if (hmc5883::calibrate(busid) == 0) { errx(0, "calibration successful"); } else { @@ -1697,38 +1645,25 @@ hmc5883_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(verb, "test")) - hmc5883::test(bus); + hmc5883::test(busid); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - hmc5883::reset(bus); + hmc5883::reset(busid); /* * Print driver information. */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - if (bus == HMC5883_BUS_ALL) { - int ret = 0; - if (hmc5883::info(HMC5883_BUS_INTERNAL)) { - ret = 1; - } - - if (hmc5883::info(HMC5883_BUS_EXTERNAL)) { - ret = 1; - } - exit(ret); - } else { - exit(hmc5883::info(bus)); - } - } + if (!strcmp(verb, "info") || !strcmp(verb, "status")) + hmc5883::info(busid); /* * Autocalibrate the scaling */ if (!strcmp(verb, "calibrate")) { - if (hmc5883::calibrate(bus) == 0) { + if (hmc5883::calibrate(busid) == 0) { errx(0, "calibration successful"); } else { diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h index 0eb7736295f5..e91e91fc0996 100644 --- a/src/drivers/hmc5883/hmc5883.h +++ b/src/drivers/hmc5883/hmc5883.h @@ -50,3 +50,4 @@ /* interface factories */ extern device::Device *HMC5883_SPI_interface(int bus) weak_function; extern device::Device *HMC5883_I2C_interface(int bus) weak_function; +typedef device::Device* (*HMC5883_constructor)(int); From 4cbc6ff60ff89a72168e007dbe6e829fdce534ec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Jan 2015 14:29:53 +1100 Subject: [PATCH 046/293] hmc5883: fixed mixup of internal and external hmc5883 I2C bus options --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 2a10b006322a..9d63479f38dd 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1305,9 +1305,9 @@ struct hmc5883_bus_option { uint8_t busnum; HMC5883 *dev; } bus_options[] = { - { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, + { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, #ifdef PX4_I2C_BUS_ONBOARD - { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, + { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, #endif #ifdef PX4_SPIDEV_HMC { HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, From 83177c6611ba81ddb675da248e68e6e478320b74 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Jan 2015 15:07:05 +1100 Subject: [PATCH 047/293] hmc5883: fixed DEVIOCGDEVICEID ioctl we need to pass the ioctl through to the bus interface thanks to Jon Challinger for noticing this bug --- src/drivers/hmc5883/hmc5883.cpp | 4 ++++ src/drivers/hmc5883/hmc5883_i2c.cpp | 5 +++++ src/drivers/hmc5883/hmc5883_spi.cpp | 5 +++++ 3 files changed, 14 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 9d63479f38dd..95fbed0ba34a 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -66,6 +66,7 @@ #include #include #include +#include #include #include @@ -725,6 +726,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) debug("MAGIOCGEXTERNAL in main driver"); return _interface->ioctl(cmd, dummy); + case DEVIOCGDEVICEID: + return _interface->ioctl(cmd, dummy); + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index 782ea62fe798..f86c1af6b18f 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -53,6 +53,7 @@ #include #include +#include #include "hmc5883.h" @@ -90,6 +91,7 @@ HMC5883_I2C_interface(int bus) HMC5883_I2C::HMC5883_I2C(int bus) : I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } HMC5883_I2C::~HMC5883_I2C() @@ -117,6 +119,9 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) return 0; } + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + default: ret = -EINVAL; } diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp index 25a2f2b40536..aec990ca884b 100644 --- a/src/drivers/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -53,6 +53,7 @@ #include #include +#include #include "hmc5883.h" #include @@ -92,6 +93,7 @@ HMC5883_SPI_interface(int bus) HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) : SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } HMC5883_SPI::~HMC5883_SPI() @@ -146,6 +148,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) return 0; } + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + default: { ret = -EINVAL; From 6603bbebe32ef36bbf939c9f085fbdfd1d9336ce Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 21 Jan 2015 16:46:04 +0900 Subject: [PATCH 048/293] batt_smbus: disable if no batt 10seconds after startup --- src/drivers/batt_smbus/batt_smbus.cpp | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index f2eaac3d37ce..16f61d089a41 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -91,6 +91,7 @@ #define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register #define BATT_SMBUS_CURRENT 0x2a ///< current register #define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz +#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup #define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC @@ -171,11 +172,13 @@ class BATT_SMBUS : public device::I2C uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; // internal variables + bool _enabled; ///< true if we have successfully connected to battery work_s _work; ///< work queue for scheduling reads RingBuffer *_reports; ///< buffer of recorded voltages, currents struct battery_status_s _last_report; ///< last published report, used for test() orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID + uint64_t _start_time; ///< system time we first attempt to communicate with battery }; namespace @@ -189,13 +192,18 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), + _enabled(false), _work{}, _reports(nullptr), _batt_topic(-1), - _batt_orb_id(nullptr) + _batt_orb_id(nullptr), + _start_time(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); + + // capture startup time + _start_time = hrt_absolute_time(); } BATT_SMBUS::~BATT_SMBUS() @@ -330,11 +338,20 @@ BATT_SMBUS::cycle_trampoline(void *arg) void BATT_SMBUS::cycle() { + // get current time + uint64_t now = hrt_absolute_time(); + + // exit without rescheduling if we have failed to find a battery after 10 seconds + if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) { + warnx("did not find smart battery"); + return; + } + // read data from sensor struct battery_status_s new_report; // set time of reading - new_report.timestamp = hrt_absolute_time(); + new_report.timestamp = now; // read voltage uint16_t tmp; @@ -375,6 +392,9 @@ BATT_SMBUS::cycle() // notify anyone waiting for data poll_notify(POLLIN); + + // record we are working + _enabled = true; } // schedule a fresh cycle call when the measurement is done From b2b6ec519b61ffb08de5f31a00ae4a2cbc6de823 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 28 Jan 2015 17:10:25 -0800 Subject: [PATCH 049/293] px4io: add safety_arm and safety_disarm commands This will be used to make updating firmware on boot for vehicles with no safety switch possible without power cycling. The startup script needs to be able to force safety on to allow the reboot to work. --- src/drivers/px4io/px4io.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 58390ba4c3b9..cbe3807f6274 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -3263,7 +3263,23 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "safety_arm")) { + int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); + if (ret != OK) { + printf("failed to arm px4io\n"); + exit(1); + } + exit(0); + } + if (!strcmp(argv[1], "safety_disarm")) { + int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); + if (ret != OK) { + printf("failed to disarm px4io\n"); + exit(1); + } + exit(0); + } if (!strcmp(argv[1], "recovery")) { @@ -3392,6 +3408,6 @@ px4io_main(int argc, char *argv[]) out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug ',\n" - "'recovery', 'limit ', 'current', 'bind', 'checkcrc',\n" + "'recovery', 'limit ', 'current', 'bind', 'checkcrc', 'safety_arm', 'safety_disarm',\n" "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); } From 0ab371603dd9df1067a802a6712112f4157a4474 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 1 Feb 2015 12:07:16 +1100 Subject: [PATCH 050/293] ms5611: allow for all 4 bus combinations in ms5611 driver this uses the same table driven approach as the hmc5883 driver, and allows for a ms5611 baro on any of the 4 bus combinations. A simple "ms5611 start" will start all baros that are found. --- src/drivers/ms5611/ms5611.cpp | 259 ++++++++++++++++-------------- src/drivers/ms5611/ms5611.h | 6 +- src/drivers/ms5611/ms5611_i2c.cpp | 18 +-- src/drivers/ms5611/ms5611_spi.cpp | 25 +-- 4 files changed, 161 insertions(+), 147 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0a793cbc97b9..770fbea98781 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -57,6 +57,7 @@ #include #include +#include #include #include @@ -68,6 +69,14 @@ #include "ms5611.h" +enum MS5611_BUS { + MS5611_BUS_ALL = 0, + MS5611_BUS_I2C_INTERNAL, + MS5611_BUS_I2C_EXTERNAL, + MS5611_BUS_SPI_INTERNAL, + MS5611_BUS_SPI_EXTERNAL +}; + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -775,15 +784,38 @@ MS5611::print_info() namespace ms5611 { -/* initialize explicitely for clarity */ -MS5611 *g_dev_ext = nullptr; -MS5611 *g_dev_int = nullptr; +/* + list of supported bus configurations + */ +struct ms5611_bus_option { + enum MS5611_BUS busid; + const char *devpath; + MS5611_constructor interface_constructor; + uint8_t busnum; + MS5611 *dev; +} bus_options[] = { +#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) + { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, +#endif +#ifdef PX4_SPIDEV_BARO + { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_SENSORS, NULL }, +#endif +#ifdef PX4_I2C_BUS_ONBOARD + { MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_I2C_BUS_EXPANSION + { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -void start(bool external_bus); -void test(bool external_bus); -void reset(bool external_bus); +bool start_bus(struct ms5611_bus_option &bus); +struct ms5611_bus_option &find_bus(enum MS5611_BUS busid); +void start(enum MS5611_BUS busid); +void test(enum MS5611_BUS busid); +void reset(enum MS5611_BUS busid); void info(); -void calibrate(unsigned altitude, bool external_bus); +void calibrate(unsigned altitude, enum MS5611_BUS busid); void usage(); /** @@ -836,89 +868,89 @@ crc4(uint16_t *n_prom) /** * Start the driver. */ -void -start(bool external_bus) +bool +start_bus(struct ms5611_bus_option &bus) { - int fd; - prom_u prom_buf; - - if (external_bus && (g_dev_ext != nullptr)) { - /* if already started, the still command succeeded */ - errx(0, "ext already started"); - } else if (!external_bus && (g_dev_int != nullptr)) { - /* if already started, the still command succeeded */ - errx(0, "int already started"); - } - - device::Device *interface = nullptr; - - /* create the driver, try SPI first, fall back to I2C if unsuccessful */ - if (MS5611_spi_interface != nullptr) - interface = MS5611_spi_interface(prom_buf, external_bus); - if (interface == nullptr && (MS5611_i2c_interface != nullptr)) - interface = MS5611_i2c_interface(prom_buf); - - if (interface == nullptr) - errx(1, "failed to allocate an interface"); + ::printf("starting bus %s\n", bus.devpath); + usleep(5000); + if (bus.dev != nullptr) + errx(1,"bus option already started"); + prom_u prom_buf; + device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); if (interface->init() != OK) { delete interface; - errx(1, "interface init failed"); + warnx("no device on bus %u", (unsigned)bus.busid); + return false; } - if (external_bus) { - g_dev_ext = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_EXT); - if (g_dev_ext == nullptr) { - delete interface; - errx(1, "failed to allocate driver"); - } - - if (g_dev_ext->init() != OK) - goto fail; - - fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); - - } else { - - g_dev_int = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_INT); - if (g_dev_int == nullptr) { - delete interface; - errx(1, "failed to allocate driver"); - } - - if (g_dev_int->init() != OK) - goto fail; - - fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); - + bus.dev = new MS5611(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; } + + int fd = open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ - - if (fd < 0) { - warnx("can't open baro device"); - goto fail; + if (fd == -1) { + errx(1, "can't open baro device"); } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - warnx("failed setting default poll rate"); - goto fail; + close(fd); + errx(1, "failed setting default poll rate"); } - exit(0); + close(fd); + return true; +} -fail: - if (g_dev_int != nullptr) { - delete g_dev_int; - g_dev_int = nullptr; - } +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +void +start(enum MS5611_BUS busid) +{ + uint8_t i; + bool started = false; - if (g_dev_ext != nullptr) { - delete g_dev_ext; - g_dev_ext = nullptr; + for (i=0; iprint_info(); - } - - if (g_dev_int) { - warnx("int:"); - g_dev_int->print_info(); + for (uint8_t i=0; iprint_info(); + } } - exit(0); } @@ -1044,19 +1063,16 @@ info() * Calculate actual MSL pressure given current altitude */ void -calibrate(unsigned altitude, bool external_bus) +calibrate(unsigned altitude, enum MS5611_BUS busid) { + struct ms5611_bus_option &bus = find_bus(busid); struct baro_report report; float pressure; float p1; int fd; - if (external_bus) { - fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); - } else { - fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); - } + fd = open(bus.devpath, O_RDONLY); if (fd < 0) err(1, "open failed (try 'ms5611 start' if the driver is not running)"); @@ -1111,6 +1127,7 @@ calibrate(unsigned altitude, bool external_bus) if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) err(1, "BAROIOCSMSLPRESSURE"); + close(fd); exit(0); } @@ -1119,7 +1136,10 @@ usage() { warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); warnx("options:"); - warnx(" -X (external bus)"); + warnx(" -X (external I2C bus)"); + warnx(" -I (intternal I2C bus)"); + warnx(" -S (external SPI bus)"); + warnx(" -s (internal SPI bus)"); } } // namespace @@ -1127,14 +1147,23 @@ usage() int ms5611_main(int argc, char *argv[]) { - bool external_bus = false; + enum MS5611_BUS busid = MS5611_BUS_ALL; int ch; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "X")) != EOF) { + while ((ch = getopt(argc, argv, "XISs")) != EOF) { switch (ch) { case 'X': - external_bus = true; + busid = MS5611_BUS_I2C_EXTERNAL; + break; + case 'I': + busid = MS5611_BUS_I2C_INTERNAL; + break; + case 'S': + busid = MS5611_BUS_SPI_EXTERNAL; + break; + case 's': + busid = MS5611_BUS_SPI_INTERNAL; break; default: ms5611::usage(); @@ -1144,29 +1173,23 @@ ms5611_main(int argc, char *argv[]) const char *verb = argv[optind]; - if (argc > optind+1) { - if (!strcmp(argv[optind+1], "-X")) { - external_bus = true; - } - } - /* * Start/load the driver. */ if (!strcmp(verb, "start")) - ms5611::start(external_bus); + ms5611::start(busid); /* * Test the driver/device. */ if (!strcmp(verb, "test")) - ms5611::test(external_bus); + ms5611::test(busid); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - ms5611::reset(external_bus); + ms5611::reset(busid); /* * Print driver information. @@ -1183,7 +1206,7 @@ ms5611_main(int argc, char *argv[]) long altitude = strtol(argv[optind+1], nullptr, 10); - ms5611::calibrate(altitude, external_bus); + ms5611::calibrate(altitude, busid); } errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index f0b3fd61d753..07d35217159a 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom); } /* namespace */ /* interface factories */ -extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) weak_function; -extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function; - +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; +extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; +typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 87d9b94a6e9f..ca651409f968 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -56,14 +56,6 @@ #include "board_config.h" -#ifdef PX4_I2C_OBDEV_MS5611 - -#ifndef PX4_I2C_BUS_ONBOARD - #define MS5611_BUS 1 -#else - #define MS5611_BUS PX4_I2C_BUS_ONBOARD -#endif - #define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ #define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ @@ -74,7 +66,7 @@ device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf); class MS5611_I2C : public device::I2C { public: - MS5611_I2C(int bus, ms5611::prom_u &prom_buf); + MS5611_I2C(uint8_t bus, ms5611::prom_u &prom_buf); virtual ~MS5611_I2C(); virtual int init(); @@ -113,12 +105,12 @@ class MS5611_I2C : public device::I2C }; device::Device * -MS5611_i2c_interface(ms5611::prom_u &prom_buf) +MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) { - return new MS5611_I2C(MS5611_BUS, prom_buf); + return new MS5611_I2C(busnum, prom_buf); } -MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) : +MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) : I2C("MS5611_I2C", nullptr, bus, 0, 400000), _prom(prom) { @@ -274,5 +266,3 @@ MS5611_I2C::_read_prom() /* calculate CRC and return success/failure accordingly */ return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; } - -#endif /* PX4_I2C_OBDEV_MS5611 */ diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 5234ce8d6b4c..554a1d659645 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -60,14 +60,14 @@ #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -#ifdef PX4_SPIDEV_BARO +#if defined(PX4_SPIDEV_BARO) || defined(PX4_SPIDEV_EXT_BARO) device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus); class MS5611_SPI : public device::SPI { public: - MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf); + MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf); virtual ~MS5611_SPI(); virtual int init(); @@ -115,20 +115,21 @@ class MS5611_SPI : public device::SPI }; device::Device * -MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) +MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) { - if (external_bus) { - #ifdef PX4_SPI_BUS_EXT - return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); - #else +#ifdef PX4_SPI_BUS_EXT + if (busnum == PX4_SPI_BUS_EXT) { +#ifdef PX4_SPIDEV_EXT_BARO + return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf); +#else return nullptr; - #endif - } else { - return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); +#endif } +#endif + return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); } -MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : +MS5611_SPI::MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf) : SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */), _prom(prom_buf) { @@ -281,4 +282,4 @@ MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) return transfer(send, recv, len); } -#endif /* PX4_SPIDEV_BARO */ +#endif /* PX4_SPIDEV_BARO || PX4_SPIDEV_EXT_BARO */ From 9ebcc43b8ef05467940034f045160c133017f0cd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 1 Feb 2015 12:08:06 +1100 Subject: [PATCH 051/293] px4fmu-v1: removed baro I2C address in board_config.h the ms5611 can be on two addresses. The driver handles this, not the board config --- src/drivers/boards/px4fmu-v1/board_config.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 188885be2d05..882ec6aa2de0 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -114,7 +114,6 @@ __BEGIN_DECLS * Note that these are unshifted addresses. */ #define PX4_I2C_OBDEV_HMC5883 0x1e -#define PX4_I2C_OBDEV_MS5611 0x76 #define PX4_I2C_OBDEV_EEPROM NOTDEFINED #define PX4_I2C_OBDEV_LED 0x55 From fba85df042842629846b06468f67218ce2a9a696 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 2 Feb 2015 07:21:27 +1100 Subject: [PATCH 052/293] ms5611: removed debug code --- src/drivers/ms5611/ms5611.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 770fbea98781..83b558183878 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -871,8 +871,6 @@ crc4(uint16_t *n_prom) bool start_bus(struct ms5611_bus_option &bus) { - ::printf("starting bus %s\n", bus.devpath); - usleep(5000); if (bus.dev != nullptr) errx(1,"bus option already started"); From c9e12a40cf3d65c19b965d238d2b708a70636ade Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Feb 2015 13:14:30 +0100 Subject: [PATCH 053/293] IO driver: Fix naming of safety command line argument --- 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 cbe3807f6274..2ae23a35c289 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -3263,19 +3263,19 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "safety_arm")) { + if (!strcmp(argv[1], "safety_off")) { int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); if (ret != OK) { - printf("failed to arm px4io\n"); + printf("failed to disable safety\n"); exit(1); } exit(0); } - if (!strcmp(argv[1], "safety_disarm")) { + if (!strcmp(argv[1], "safety_on")) { int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); if (ret != OK) { - printf("failed to disarm px4io\n"); + printf("failed to enable safety\n"); exit(1); } exit(0); @@ -3408,6 +3408,6 @@ px4io_main(int argc, char *argv[]) out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug ',\n" - "'recovery', 'limit ', 'current', 'bind', 'checkcrc', 'safety_arm', 'safety_disarm',\n" + "'recovery', 'limit ', 'current', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n" "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); } From 257a836312b53deb095bc26703a53fd4c5d45ca7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Feb 2015 18:38:14 +1100 Subject: [PATCH 054/293] systemcmds: added usb_connected command this is used in APM startup to see if we should start nsh on usb when no microSD is inserted --- src/systemcmds/usb_connected/module.mk | 37 +++++++++++++ src/systemcmds/usb_connected/usb_connected.c | 56 ++++++++++++++++++++ 2 files changed, 93 insertions(+) create mode 100644 src/systemcmds/usb_connected/module.mk create mode 100644 src/systemcmds/usb_connected/usb_connected.c diff --git a/src/systemcmds/usb_connected/module.mk b/src/systemcmds/usb_connected/module.mk new file mode 100644 index 000000000000..22ee1300d2a0 --- /dev/null +++ b/src/systemcmds/usb_connected/module.mk @@ -0,0 +1,37 @@ +############################################################################ +# +# 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. +# +############################################################################ + +MODULE_COMMAND = usb_connected +SRCS = usb_connected.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/usb_connected/usb_connected.c b/src/systemcmds/usb_connected/usb_connected.c new file mode 100644 index 000000000000..98bbbc4be958 --- /dev/null +++ b/src/systemcmds/usb_connected/usb_connected.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 usb_connected.c + * + * utility to check if USB is connected. Used in startup scripts + * + * @author Andrew Tridgell + */ + +#include +#include +#include +#include +#include +#include +#include + +__EXPORT int usb_connected_main(int argc, char *argv[]); + +int +usb_connected_main(int argc, char *argv[]) +{ + return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0:1; +} From 64e52b13939138c69441c0389fd4ea46ac44cc9c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 11 Feb 2015 16:22:31 +1100 Subject: [PATCH 055/293] l3gd20: fixed "l3gd20 test" don't reset after test, and leave us in correct polling mode --- src/drivers/l3gd20/l3gd20.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 08bc1fead857..ffd14f3f8c42 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1231,11 +1231,12 @@ test() warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "reset to default polling"); + close(fd_gyro); /* XXX add poll-rate tests here too */ - - reset(); errx(0, "PASS"); } From 93ca5171f029559e374d18f6da527bc5c013f6a2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 11 Feb 2015 17:01:34 +1100 Subject: [PATCH 056/293] l3gd20: checking status only makes sense if we have DRDY it makes no sense on the external SPI bus --- src/drivers/l3gd20/l3gd20.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index ffd14f3f8c42..fafb96596df7 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -969,7 +969,7 @@ L3GD20::measure() transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); #if L3GD20_USE_DRDY - if ((raw_report.status & 0xF) != 0xF) { + if (_bus == PX4_SPI_BUS_SENSORS && (raw_report.status & 0xF) != 0xF) { /* we waited for DRDY, but did not see DRDY on all axes when we captured. That means a transfer error of some sort From 7799dc5c3776c238ded4d4556a40d0e7f5399e97 Mon Sep 17 00:00:00 2001 From: parallels Date: Tue, 27 Jan 2015 00:41:12 +0100 Subject: [PATCH 057/293] trone: added support for WHOAMI register --- src/drivers/trone/trone.cpp | 103 +++++++++++++++++++++--------------- 1 file changed, 59 insertions(+), 44 deletions(-) diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index cf3546669432..5f327cf7f485 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -72,14 +72,17 @@ #include /* Configuration Constants */ -#define TRONE_BUS PX4_I2C_BUS_EXPANSION -#define TRONE_BASEADDR 0x30 /* 7-bit address */ -#define TRONE_DEVICE_PATH "/dev/trone" +#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 */ +#define TRONE_WHO_AM_I_REG 0x01 /* Who am I test register */ +#define TRONE_WHO_AM_I_REG_VAL 0xA1 + /* Device limits */ #define TRONE_MIN_DISTANCE (0.20f) #define TRONE_MAX_DISTANCE (14.00f) @@ -182,40 +185,40 @@ class TRONE : public device::I2C }; 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 + 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 }; static uint8_t crc8(uint8_t *p, uint8_t len) { - uint16_t i; - uint16_t crc = 0x0; + uint16_t i; + uint16_t crc = 0x0; - while (len--) { - i = (crc ^ *p++) & 0xFF; - crc = (crc_table[i] ^ (crc << 8)) & 0xFF; - } + while (len--) { + i = (crc ^ *p++) & 0xFF; + crc = (crc_table[i] ^ (crc << 8)) & 0xFF; + } - return crc & 0xFF; + return crc & 0xFF; } /* @@ -308,7 +311,20 @@ TRONE::init() int TRONE::probe() { - return measure(); + uint8_t who_am_i=0; + + const uint8_t cmd = TRONE_WHO_AM_I_REG; + if (transfer(&cmd, 1, &who_am_i, 1) == OK && who_am_i == TRONE_WHO_AM_I_REG_VAL) { + // it is responding correctly to a WHO_AM_I + return measure(); + } + + debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n", + (unsigned)who_am_i, + TRONE_WHO_AM_I_REG_VAL); + + // not found on any address + return -EIO; } void @@ -534,9 +550,9 @@ TRONE::measure() int TRONE::collect() { - int ret = -EIO; + int ret = -EIO; - /* read from the sensor */ + /* read from the sensor */ uint8_t val[3] = {0, 0, 0}; perf_begin(_sample_perf); @@ -643,13 +659,12 @@ TRONE::cycle() * 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)); + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL)); return; } @@ -665,10 +680,10 @@ TRONE::cycle() /* schedule a fresh cycle call when the measurement is done */ work_queue(HPWORK, - &_work, - (worker_t)&TRONE::cycle_trampoline, - this, - USEC2TICK(TRONE_CONVERSION_INTERVAL)); + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + USEC2TICK(TRONE_CONVERSION_INTERVAL)); } void From 471eb1401618b75bd730cb610dc2d0f54e94fe1a Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Fri, 6 Feb 2015 13:18:44 +1100 Subject: [PATCH 058/293] hmc5883: Fix for Issue1858 detection of MAG on Int/Ext I2C Bus. --- src/drivers/hmc5883/hmc5883_i2c.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index f86c1af6b18f..b13f1fca8feb 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -113,11 +113,17 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) switch (operation) { case MAGIOCGEXTERNAL: +// On PX4v1 the MAG can be on an internal I2C +// On everything else its always external +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 if (_bus == PX4_I2C_BUS_EXPANSION) { return 1; } else { return 0; } +#else + return 1; +#endif case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); From e6126fd7daa820a14c8bdba3bff2a4fdb0fe5b9b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Feb 2015 11:31:35 +1100 Subject: [PATCH 059/293] batt_smbus: run I2C at 100kHz --- src/drivers/batt_smbus/batt_smbus.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 16f61d089a41..a0ff0051c63b 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -191,7 +191,7 @@ void batt_smbus_usage(); extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : - I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), + I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 100000), _enabled(false), _work{}, _reports(nullptr), From ff6152466c85e66ffab23f4e104f7942d1e4824c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Feb 2015 11:32:26 +1100 Subject: [PATCH 060/293] px4flow: run I2C at 100kHz --- 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 9c9c1b0f819a..5a0f64c07737 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -81,7 +81,7 @@ #define PX4FLOW_REG 0x16 ///< Measure Register 22 #define PX4FLOW_CONVERSION_INTERVAL 20000 ///< in microseconds! 20000 = 50 Hz 100000 = 10Hz -#define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed +#define PX4FLOW_I2C_MAX_BUS_SPEED 100000 ///< 100 KHz maximum speed /* oddly, ERROR is not defined for c++ */ #ifdef ERROR From 58cbeed95814972f2da731bcba7423686a7e767e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Feb 2015 11:32:39 +1100 Subject: [PATCH 061/293] ms5611: run I2C at 100kHz --- src/drivers/ms5611/ms5611_i2c.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index ca651409f968..31d56bf85f54 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -111,7 +111,7 @@ MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) } MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) : - I2C("MS5611_I2C", nullptr, bus, 0, 400000), + I2C("MS5611_I2C", nullptr, bus, 0, 100000), _prom(prom) { } From 9d9beb0888abb1d965358954b591a6373301fe37 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Feb 2015 11:34:19 +1100 Subject: [PATCH 062/293] hmc5883: run I2C at 100kHz this actually reduces timing glitches in ArduPilot significantly, as the I2C transfer has enough time between interrupts for other tasks to get some work done. Changing just this bus speed down to 100kHz reduces the number of timing misses for ArduCopter on Pixhawk by a factor of 3x --- src/drivers/hmc5883/hmc5883_i2c.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index b13f1fca8feb..d5e47661d476 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -89,7 +89,7 @@ HMC5883_I2C_interface(int bus) } HMC5883_I2C::HMC5883_I2C(int bus) : - I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000) + I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 100000) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } From 6e00c4c6994b28dd23d604698c62e23210024714 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Feb 2015 20:08:51 +1100 Subject: [PATCH 063/293] hmc5883: fixed build errors with gcc 2.7.4 and -O3 --- src/drivers/hmc5883/hmc5883.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index ce2105eaec18..63b35fb19c61 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -477,7 +477,7 @@ int HMC5883::set_range(unsigned range) if (OK != ret) perf_count(_comms_errors); - uint8_t range_bits_in; + uint8_t range_bits_in = 0; ret = read_reg(ADDR_CONF_B, range_bits_in); if (OK != ret) @@ -495,7 +495,7 @@ void HMC5883::check_range(void) { int ret; - uint8_t range_bits_in; + uint8_t range_bits_in = 0; ret = read_reg(ADDR_CONF_B, range_bits_in); if (OK != ret) { perf_count(_comms_errors); @@ -518,7 +518,7 @@ void HMC5883::check_conf(void) { int ret; - uint8_t conf_reg_in; + uint8_t conf_reg_in = 0; ret = read_reg(ADDR_CONF_A, conf_reg_in); if (OK != ret) { perf_count(_comms_errors); @@ -1215,7 +1215,7 @@ int HMC5883::set_excitement(unsigned enable) if (OK != ret) perf_count(_comms_errors); - uint8_t conf_reg_ret; + uint8_t conf_reg_ret = 0; read_reg(ADDR_CONF_A, conf_reg_ret); //print_info(); From 55fff0dfcecbeb3ba169c17d84f3da83b92c349f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Feb 2015 20:09:05 +1100 Subject: [PATCH 064/293] batt_smbus: fixed device path --- src/drivers/batt_smbus/batt_smbus.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index a0ff0051c63b..0e3aa670f385 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -191,7 +191,7 @@ void batt_smbus_usage(); extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : - I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 100000), + I2C("batt_smbus", BATT_SMBUS0_DEVICE_PATH, bus, batt_smbus_addr, 100000), _enabled(false), _work{}, _reports(nullptr), From 3104fd9e3be07317140af509f0213a64929f33a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Feb 2015 21:38:21 +1100 Subject: [PATCH 065/293] build: avoid wiping an existing PYTHONPATH variable --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 05e0d90a25a0..ab8c614e1407 100644 --- a/Makefile +++ b/Makefile @@ -238,11 +238,11 @@ GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src .PHONY: generateuorbtopicheaders generateuorbtopicheaders: @$(ECHO) "Generating uORB topic headers" - $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR)) @$(ECHO) "Generating multiplatform uORB topic wrapper headers" - $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ -d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX)) # clean up temporary files From e2adbba205212f9cfc4a00a422e80a0954a317bc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Feb 2015 10:22:55 +1100 Subject: [PATCH 066/293] i2c: prevent double free of _dev pointer this caused heap corruption --- src/drivers/device/i2c.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/i2c.cpp b/src/drivers/device/i2c.cpp index 33bb90fc97bf..092ecd144075 100644 --- a/src/drivers/device/i2c.cpp +++ b/src/drivers/device/i2c.cpp @@ -74,8 +74,10 @@ I2C::I2C(const char *name, I2C::~I2C() { - if (_dev) + if (_dev) { up_i2cuninitialize(_dev); + _dev = nullptr; + } } int @@ -118,6 +120,7 @@ I2C::init() // is smaller than the bus frequency if (_bus_clocks[bus_index] > _frequency) { (void)up_i2cuninitialize(_dev); + _dev = nullptr; log("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); ret = -EINVAL; @@ -164,6 +167,7 @@ I2C::init() out: if ((ret != OK) && (_dev != nullptr)) { up_i2cuninitialize(_dev); + _dev = nullptr; } return ret; } From 487a3a4926bdf54759496d89aeda4f2ed1cec07b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Feb 2015 10:43:49 +1100 Subject: [PATCH 067/293] Tools: improve python package error message --- Tools/px_generate_uorb_topic_headers.py | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index ce2f29f3f101..8d6a12316320 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -46,10 +46,20 @@ try: import genmsg.template_tools except ImportError as e: - print("Package empy not installed. Please run 'sudo apt-get install" - " python-empy' on a Debian/Ubuntu system, 'sudo pip install" - " empy' on a Mac OS system or 'easy_install empy' on" - " a Windows system.") + print(''' +Required python packages not installed. + +On a Debian/Ubuntu syystem please run: + + sudo apt-get install python-empy + sudo pip install catkin_pkg + +On MacOS please run: + sudo pip install empy catkin_pkg + +On Windows please run: + easy_install empy catkin_pkg +''') exit(1) __author__ = "Thomas Gubler" From ee41faec27964d82853b52a2d0c0d3b430020a96 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Feb 2015 14:14:39 +1100 Subject: [PATCH 068/293] PX4: better error msg in python uorb generation error --- Tools/px_generate_uorb_topic_headers.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 8d6a12316320..c7ed72d58287 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -46,6 +46,7 @@ try: import genmsg.template_tools except ImportError as e: + print("python import error: ", e) print(''' Required python packages not installed. From 8d11607b30dc142f8ce7da7d1a79c060d6cf441a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 19 Feb 2015 23:28:04 +0900 Subject: [PATCH 069/293] batt_smbus: reverse reported current smart batteries report a negative current when being discharged --- src/drivers/batt_smbus/batt_smbus.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 0e3aa670f385..94ddfd34d689 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -368,8 +368,7 @@ BATT_SMBUS::cycle() uint8_t buff[4]; if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { - new_report.current_a = (float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | - (uint32_t)buff[0])) / 1000.0f; + new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | (uint32_t)buff[0])) / 1000.0f; } // publish to orb From 5341001bf3b95b2c250a224c9d96037b75e2ca0b Mon Sep 17 00:00:00 2001 From: Evan Slatyer Date: Fri, 20 Feb 2015 11:53:42 +1100 Subject: [PATCH 070/293] drivers: added header for pwm_input --- src/drivers/drv_pwm_input.h | 51 +++++++++++++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 src/drivers/drv_pwm_input.h diff --git a/src/drivers/drv_pwm_input.h b/src/drivers/drv_pwm_input.h new file mode 100644 index 000000000000..778d9fcf5b47 --- /dev/null +++ b/src/drivers/drv_pwm_input.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include +#include + +#define PWMIN0_DEVICE_PATH "/dev/pwmin0" + +__BEGIN_DECLS + +/* + * Initialise the timer + */ +__EXPORT extern int pwm_input_main(int argc, char * argv[]); + +__END_DECLS From ace5bbf27df1a98406c8b8a96cec6cdc7515b845 Mon Sep 17 00:00:00 2001 From: Evan Slatyer Date: Fri, 20 Feb 2015 12:11:07 +1100 Subject: [PATCH 071/293] pwm_input: added uORB message structure --- msg/pwm_input.msg | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 msg/pwm_input.msg diff --git a/msg/pwm_input.msg b/msg/pwm_input.msg new file mode 100644 index 000000000000..beb82021d267 --- /dev/null +++ b/msg/pwm_input.msg @@ -0,0 +1,5 @@ + +uint64 timestamp # Microseconds since system boot +uint64 error_count +uint32 pulse_width # Pulse width, timer counts +uint32 period # Period, timer counts From d105d8c879423b4b330ded8b73b5f02b80007e54 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 21 Feb 2015 18:15:08 +1100 Subject: [PATCH 072/293] fmuv2: setup for PWM input on timer4, channel 2 --- src/drivers/boards/px4fmu-v2/board_config.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 2fd8bc724a76..273af1023a6e 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -207,6 +207,11 @@ __BEGIN_DECLS #define HRT_TIMER 8 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL 2 +#define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 + /**************************************************************************************************** * Public Types ****************************************************************************************************/ From 5fe02c9a6bd33e9a368d7cfdb9889ca0b9ac2934 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 21 Feb 2015 18:15:57 +1100 Subject: [PATCH 073/293] pwm_input: added PWM input driver this allows for input of PWM signals on the FMUv2 aux5 pins --- src/drivers/pwm_input/module.mk | 41 ++ src/drivers/pwm_input/pwm_input.cpp | 603 ++++++++++++++++++++++++++++ 2 files changed, 644 insertions(+) create mode 100644 src/drivers/pwm_input/module.mk create mode 100644 src/drivers/pwm_input/pwm_input.cpp diff --git a/src/drivers/pwm_input/module.mk b/src/drivers/pwm_input/module.mk new file mode 100644 index 000000000000..04f04d6cbc23 --- /dev/null +++ b/src/drivers/pwm_input/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the PWM input driver. +# + +MODULE_COMMAND = pwm_input + +SRCS = pwm_input.cpp + diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp new file mode 100644 index 000000000000..2d771266c052 --- /dev/null +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -0,0 +1,603 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 pwm_input.cpp + * + * PWM input driver based on earlier driver from Evan Slatyer, + * which in turn was based on drv_hrt.c + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "chip.h" +#include "up_internal.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_gpio.h" +#include "stm32_tim.h" +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#if HRT_TIMER == PWMIN_TIMER +#error cannot share timer between HRT and PWMIN +#endif + +#if !defined(GPIO_PWM_IN) || !defined(PWMIN_TIMER) || !defined(PWMIN_TIMER_CHANNEL) +#error PWMIN defines are needed in board_config.h for this board +#endif + +/* PWMIN configuration */ +#if PWMIN_TIMER == 1 +# define PWMIN_TIMER_BASE STM32_TIM1_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB2ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1CC +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM1_CLKIN +#elif PWMIN_TIMER == 2 +# define PWMIN_TIMER_BASE STM32_TIM2_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM2 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM2_CLKIN +#elif PWMIN_TIMER == 3 +# define PWMIN_TIMER_BASE STM32_TIM3_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB1ENR_TIM3EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM3 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM3_CLKIN +#elif PWMIN_TIMER == 4 +# define PWMIN_TIMER_BASE STM32_TIM4_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB1ENR_TIM4EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM4 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM4_CLKIN +#elif PWMIN_TIMER == 5 +# define PWMIN_TIMER_BASE STM32_TIM5_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM5 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM5_CLKIN +#elif PWMIN_TIMER == 8 +# define PWMIN_TIMER_BASE STM32_TIM8_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB2ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM8EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM8CC +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM8_CLKIN +#elif PWMIN_TIMER == 9 +# define PWMIN_TIMER_BASE STM32_TIM9_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1BRK +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM9_CLKIN +#elif PWMIN_TIMER == 10 +# define PWMIN_TIMER_BASE STM32_TIM10_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1UP +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM10_CLKIN +#elif PWMIN_TIMER == 11 +# define PWMIN_TIMER_BASE STM32_TIM11_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM11_CLKIN +#elif PWMIN_TIMER == 12 +# define PWMIN_TIMER_BASE STM32_TIM12_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM12EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM12_CLKIN +#else +# error PWMIN_TIMER must be a value between 1 and 12 +#endif + +/* + * HRT clock must be at least 1MHz + */ +#if PWMIN_TIMER_CLOCK <= 1000000 +# error PWMIN_TIMER_CLOCK must be greater than 1MHz +#endif + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(PWMIN_TIMER_BASE + _reg)) + +#define rCR1 REG(STM32_GTIM_CR1_OFFSET) +#define rCR2 REG(STM32_GTIM_CR2_OFFSET) +#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +#define rDIER REG(STM32_GTIM_DIER_OFFSET) +#define rSR REG(STM32_GTIM_SR_OFFSET) +#define rEGR REG(STM32_GTIM_EGR_OFFSET) +#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +#define rCCER REG(STM32_GTIM_CCER_OFFSET) +#define rCNT REG(STM32_GTIM_CNT_OFFSET) +#define rPSC REG(STM32_GTIM_PSC_OFFSET) +#define rARR REG(STM32_GTIM_ARR_OFFSET) +#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +#define rDCR REG(STM32_GTIM_DCR_OFFSET) +#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) + +/* + * Specific registers and bits used by HRT sub-functions + */ +#if PWMIN_TIMER_CHANNEL == 1 + #define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */ + #define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */ + #define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */ + #define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */ + #define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT)) + #define CCMR2_PWMIN 0 + #define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) + #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) + #define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT) + #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#elif PWMIN_TIMER_CHANNEL == 2 + #define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */ + #define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */ + #define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */ + #define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */ + #define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT)) + #define CCMR2_PWMIN 0 + #define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) + #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) + #define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT) + #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#else + #error PWMIN_TIMER_CHANNEL must be either 1 and 2. +#endif + + +class PWMIN : device::CDev +{ +public: + PWMIN(); + virtual ~PWMIN(); + + virtual int init(); + virtual int open(struct file *filp); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + void _publish(uint16_t status, uint32_t period, uint32_t pulse_width); + void _print_info(void); + +private: + uint32_t error_count; + uint32_t pulses_captured; + uint32_t last_period; + uint32_t last_width; + RingBuffer *reports; + bool timer_started; + + void timer_init(void); +}; + +static int pwmin_tim_isr(int irq, void *context); +static void pwmin_start(void); +static void pwmin_info(void); +static void pwmin_test(void); +static void pwmin_reset(void); + +static PWMIN *g_dev; + +PWMIN::PWMIN() : + CDev("pwmin", PWMIN0_DEVICE_PATH), + error_count(0), + pulses_captured(0), + last_period(0), + last_width(0), + reports(nullptr), + timer_started(false) +{ +} + +PWMIN::~PWMIN() +{ + if (reports != nullptr) + delete reports; +} + +/* + initialise the driver. This doesn't actually start the timer (that + is done on open). We don't start the timer to allow for this driver + to be started in init scripts when the user may be using the input + pin as PWM output + */ +int +PWMIN::init() +{ + // we just register the device in /dev, and only actually + // activate the timer when requested to when the device is opened + CDev::init(); + + reports = new RingBuffer(2, sizeof(struct pwm_input_s)); + if (reports == nullptr) { + return -ENOMEM; + } + + return OK; +} + +/* + * Initialise the timer we are going to use. + */ +void PWMIN::timer_init(void) +{ + // run with interrupts disabled in case the timer is already + // setup. We don't want it firing while we are doing the setup + irqstate_t flags = irqsave(); + stm32_configgpio(GPIO_PWM_IN); + + /* claim our interrupt vector */ + irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr); + + /* Clear no bits, set timer enable bit.*/ + modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); + + /* disable and configure the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = DIER_PWMIN_A; + rCCER = 0; /* unlock CCMR* registers */ + rCCMR1 = CCMR1_PWMIN; + rCCMR2 = CCMR2_PWMIN; + rSMCR = SMCR_PWMIN_1; /* Set up mode */ + rSMCR = SMCR_PWMIN_2; /* Enable slave mode controller */ + rCCER = CCER_PWMIN; + rDCR = 0; + + // for simplicity scale by the clock in MHz. This gives us + // readings in microseconds which is typically what is needed + // for a PWM input driver + uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL; + + /* + * define the clock speed. We want the highest possible clock + * speed that avoids overflows. + */ + rPSC = prescaler - 1; + + /* run the full span of the counter. All timers can handle + * uint16 */ + rARR = UINT16_MAX; + + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; + + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; + + /* enable interrupts */ + up_enable_irq(PWMIN_TIMER_VECTOR); + + irqrestore(flags); + + timer_started = true; +} + +/* + hook for open of the driver. We start the timer at this point, then + leave it running + */ +int +PWMIN::open(struct file *filp) +{ + if (g_dev == nullptr) { + return -EIO; + } + int ret = CDev::open(filp); + if (ret == OK && !timer_started) { + g_dev->timer_init(); + } + return ret; +} + + +/* + handle ioctl requests + */ +int +PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 500)) + 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: + /* user has asked for the timer to be reset. This may + be needed if the pin was used for a different + purpose (such as PWM output) + */ + timer_init(); + return OK; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + + +/* + read some samples from the device + */ +ssize_t +PWMIN::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct pwm_input_s); + struct pwm_input_s *buf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + while (count--) { + if (reports->get(buf)) { + ret += sizeof(struct pwm_input_s); + buf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; +} + +/* + publish some data from the ISR in the ring buffer + */ +void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width) +{ + /* if we missed an edge, we have to give up */ + if (status & SR_OVF_PWMIN) { + error_count++; + return; + } + + struct pwm_input_s pwmin_report; + pwmin_report.timestamp = hrt_absolute_time(); + pwmin_report.error_count = error_count; + pwmin_report.period = period; + pwmin_report.pulse_width = pulse_width; + + reports->force(&pwmin_report); + + last_period = period; + last_width = pulse_width; + pulses_captured++; +} + +/* + print information on the last captured + */ +void PWMIN::_print_info(void) +{ + if (!timer_started) { + printf("timer not started - try the 'test' command\n"); + } else { + printf("count=%u period=%u width=%u\n", + (unsigned)pulses_captured, + (unsigned)last_period, + (unsigned)last_width); + } +} + + +/* + Handle the interupt, gathering pulse data + */ +static int pwmin_tim_isr(int irq, void *context) +{ + uint16_t status = rSR; + uint32_t period = rCCR_PWMIN_A; + uint32_t pulse_width = rCCR_PWMIN_B; + + /* ack the interrupts we just read */ + rSR = 0; + + if (g_dev != nullptr) { + g_dev->_publish(status, period, pulse_width); + } + + return OK; +} + +/* + start the driver + */ +static void pwmin_start(void) +{ + if (g_dev != nullptr) { + printf("driver already started\n"); + exit(1); + } + g_dev = new PWMIN(); + if (g_dev == nullptr) { + errx(1, "driver allocation failed"); + } + if (g_dev->init() != OK) { + errx(1, "driver init failed"); + } + exit(0); +} + +/* + test the driver + */ +static void pwmin_test(void) +{ + int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open device"); + } + uint64_t start_time = hrt_absolute_time(); + + printf("Showing samples for 5 seconds\n"); + + while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) { + struct pwm_input_s buf; + if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) { + printf("period=%u width=%u error_count=%u\n", + (unsigned)buf.period, + (unsigned)buf.pulse_width, + (unsigned)buf.error_count); + } + } + close(fd); + exit(0); +} + +/* + reset the timer + */ +static void pwmin_reset(void) +{ + int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open device"); + } + if (ioctl(fd, SENSORIOCRESET, 0) != OK) { + errx(1, "reset failed"); + } + close(fd); + exit(0); +} + +/* + show some information on the driver + */ +static void pwmin_info(void) +{ + if (g_dev == nullptr) { + printf("driver not started\n"); + exit(1); + } + g_dev->_print_info(); + exit(0); +} + + +/* + driver entry point + */ +int pwm_input_main(int argc, char * argv[]) +{ + const char *verb = argv[1]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + pwmin_start(); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + pwmin_info(); + } + + /* + * print test results + */ + if (!strcmp(verb, "test")) { + pwmin_test(); + } + + /* + * reset the timer + */ + if (!strcmp(verb, "reset")) { + pwmin_reset(); + } + + errx(1, "unrecognized command, try 'start', 'info', 'reset' or 'test'"); + return 0; +} From dd094222063c94cce96ffa09a4f0f8c7f1fdb4a0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 23 Feb 2015 13:22:30 +1100 Subject: [PATCH 074/293] px4fmu: added "mode_pwm4" startup mode this is the default mode ArduPilot uses, and by setting it in the init script we avoid any pin activity on the two GPIO pins during boot --- src/drivers/px4fmu/fmu.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 5acff70b5fe4..cf865782b571 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1527,6 +1527,7 @@ enum PortMode { PORT_GPIO_AND_SERIAL, PORT_PWM_AND_SERIAL, PORT_PWM_AND_GPIO, + PORT_PWM4, }; PortMode g_port_mode; @@ -1562,6 +1563,13 @@ fmu_new_mode(PortMode new_mode) #endif break; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case PORT_PWM4: + /* select 4-pin PWM mode */ + servo_mode = PX4FMU::MODE_4PWM; + break; +#endif + /* mixed modes supported on v1 board only */ #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) @@ -1834,6 +1842,10 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + } else if (!strcmp(verb, "mode_pwm4")) { + new_mode = PORT_PWM4; +#endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) } else if (!strcmp(verb, "mode_serial")) { From 0eb0e382851a74992b620ea0742edbb97bd63872 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Feb 2015 10:19:15 +1100 Subject: [PATCH 075/293] bl_update: fixed stat() check --- src/systemcmds/bl_update/bl_update.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c index ec9269d39d58..02d2d6f3cf2e 100644 --- a/src/systemcmds/bl_update/bl_update.c +++ b/src/systemcmds/bl_update/bl_update.c @@ -74,7 +74,7 @@ bl_update_main(int argc, char *argv[]) struct stat s; - if (!stat(argv[1], &s)) + if (stat(argv[1], &s) != 0) err(1, "stat %s", argv[1]); /* sanity-check file size */ @@ -214,4 +214,4 @@ setopt(void) errx(1, "option bits setting failed; readback 0x%04x", *optcr); -} \ No newline at end of file +} From 83bcac6a22fd18a1786930daa721dbfd41955c5e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Dec 2014 15:40:08 +0900 Subject: [PATCH 076/293] OreoLED: driver for attiny88 based LED controller --- src/drivers/drv_oreoled.h | 146 +++++++ src/drivers/oreoled/module.mk | 8 + src/drivers/oreoled/oreoled.cpp | 651 ++++++++++++++++++++++++++++++++ 3 files changed, 805 insertions(+) create mode 100644 src/drivers/drv_oreoled.h create mode 100644 src/drivers/oreoled/module.mk create mode 100644 src/drivers/oreoled/oreoled.cpp diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h new file mode 100644 index 000000000000..bc53f04a541e --- /dev/null +++ b/src/drivers/drv_oreoled.h @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_oreoled.h + * + * Oreo led device API + */ + +#pragma once + +#include +#include + +/* oreoled device path */ +#define OREOLED0_DEVICE_PATH "/dev/oreoled0" + +/* + * ioctl() definitions + */ + +#define _OREOLEDIOCBASE (0x2d00) +#define _OREOLEDIOC(_n) (_IOC(_OREOLEDIOCBASE, _n)) + +/** set constant RGB values */ +#define OREOLED_SET_RGB _OREOLEDIOC(1) + +/** run macro */ +#define OREOLED_RUN_MACRO _OREOLEDIOC(2) + +/* Oreo LED driver supports up to 4 leds */ +#define OREOLED_NUM_LEDS 4 + +/* instance of 0xff means apply to all instances */ +#define OREOLED_ALL_INSTANCES 0xff + +/* maximum command length that can be sent to LEDs */ +#define OREOLED_CMD_LENGTH_MAX 10 + +/* enum passed to OREOLED_SET_MODE ioctl() + * defined by hardware */ +enum oreoled_pattern { + OREOLED_PATTERN_OFF = 0, + OREOLED_PATTERN_SINE = 1, + OREOLED_PATTERN_SOLID = 2, + OREOLED_PATTERN_SIREN = 3, + OREOLED_PATTERN_STROBE = 4, + OREOLED_PATTERN_FADEIN = 5, + OREOLED_PATTERN_FADEOUT = 6, + OREOLED_PATTERN_PARAMUPDATE = 7, + OREOLED_PATTERN_ENUM_COUNT +}; + +/* enum passed to OREOLED_SET_MODE ioctl() + * defined by hardware */ +enum oreoled_param { + OREOLED_PARAM_BIAS_RED = 0, + OREOLED_PARAM_BIAS_GREEN = 1, + OREOLED_PARAM_BIAS_BLUE = 2, + OREOLED_PARAM_AMPLITUDE_RED = 3, + OREOLED_PARAM_AMPLITUDE_GREEN = 4, + OREOLED_PARAM_AMPLITUDE_BLUE = 5, + OREOLED_PARAM_PERIOD = 6, + OREOLED_PARAM_REPEAT = 7, + OREOLED_PARAM_PHASEOFFSET = 8, + OREOLED_PARAM_MACRO = 9, + OREOLED_PARAM_ENUM_COUNT +}; + +/* enum of available macros + * defined by hardware */ +enum oreoled_macro { + OREOLED_PARAM_MACRO_RESET = 0, + OREOLED_PARAM_MACRO_COLOUR_CYCLE = 1, + OREOLED_PARAM_MACRO_BREATH = 2, + OREOLED_PARAM_MACRO_STROBE = 3, + OREOLED_PARAM_MACRO_FADEIN = 4, + OREOLED_PARAM_MACRO_FADEOUT = 5, + OREOLED_PARAM_MACRO_RED = 6, + OREOLED_PARAM_MACRO_GREEN = 7, + OREOLED_PARAM_MACRO_BLUE = 8, + OREOLED_PARAM_MACRO_YELLOW = 9, + OREOLED_PARAM_MACRO_WHITE = 10, + OREOLED_PARAM_MACRO_ENUM_COUNT +}; + +/* + structure passed to OREOLED_SET_RGB ioctl() + Note that the driver scales the brightness to 0 to 255, regardless + of the hardware scaling + */ +typedef struct { + uint8_t instance; + oreoled_pattern pattern; + uint8_t red; + uint8_t green; + uint8_t blue; +} oreoled_rgbset_t; + +/* + structure passed to OREOLED_RUN_MACRO ioctl() + */ +typedef struct { + uint8_t instance; + oreoled_macro macro; +} oreoled_macrorun_t; + +/* + structure passed to send_bytes method (only used for testing) + */ +typedef struct { + uint8_t led_num; + uint8_t num_bytes; + uint8_t buff[OREOLED_CMD_LENGTH_MAX]; +} oreoled_cmd_t; + diff --git a/src/drivers/oreoled/module.mk b/src/drivers/oreoled/module.mk new file mode 100644 index 000000000000..96afdd5fdc3b --- /dev/null +++ b/src/drivers/oreoled/module.mk @@ -0,0 +1,8 @@ +# +# Oreo LED driver +# + +MODULE_COMMAND = oreoled +SRCS = oreoled.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp new file mode 100644 index 000000000000..afd20275a90f --- /dev/null +++ b/src/drivers/oreoled/oreoled.cpp @@ -0,0 +1,651 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Randy Mackay + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file oreoled.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#define OREOLED_NUM_LEDS 4 ///< maximum number of LEDs the oreo led driver can support +#define OREOLED_BASE_I2C_ADDR 0x68 ///< base i2c address (7-bit) +#define OREOLED_TIMEOUT_MS 10000000U ///< timeout looking for battery 10seconds after startup +#define OREOLED_GENERALCALL_US 4000000U ///< general call sent every 4 seconds +#define OREOLED_GENERALCALL_CMD 0x00 ///< general call command sent at regular intervals + +#define OREOLED_STARTUP_INTERVAL_US (1000000U / 10U) ///< time in microseconds, measure at 10hz +#define OREOLED_UPDATE_INTERVAL_US (1000000U / 50U) ///< time in microseconds, measure at 10hz + +#define OREOLED_CMD_QUEUE_SIZE 10 ///< up to 10 messages can be queued up to send to the LEDs + +class OREOLED : public device::I2C +{ +public: + OREOLED(int bus, int i2c_addr); + virtual ~OREOLED(); + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /* send general call on I2C bus to syncronise all LEDs */ + int send_general_call(); + + /* send cmd to an LEDs (used for testing only) */ + int send_cmd(oreoled_cmd_t sb); + +private: + + /** + * Start periodic updates to the LEDs + */ + void start(); + + /** + * Stop periodic updates to the LEDs + */ + void stop(); + + /** + * static function that is called by worker queue + */ + static void cycle_trampoline(void *arg); + + /** + * update the colours displayed by the LEDs + */ + void cycle(); + + /* internal variables */ + work_s _work; ///< work queue for scheduling reads + bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED + uint8_t _num_healthy; ///< number of healthy LEDs + RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs + uint64_t _last_gencall; + uint64_t _start_time; ///< system time we first attempt to communicate with battery +}; + +/* for now, we only support one OREOLED */ +namespace +{ +OREOLED *g_oreoled = nullptr; +} + +void oreoled_usage(); + +extern "C" __EXPORT int oreoled_main(int argc, char *argv[]); + +/* constructor */ +OREOLED::OREOLED(int bus, int i2c_addr) : + I2C("oreoled", OREOLED0_DEVICE_PATH, bus, i2c_addr, 100000), + _work{}, + _num_healthy(0), + _cmd_queue(nullptr), + _last_gencall(0) +{ + /* initialise to unhealthy */ + memset(_healthy, 0, sizeof(_healthy)); + + /* capture startup time */ + _start_time = hrt_absolute_time(); +} + +/* destructor */ +OREOLED::~OREOLED() +{ + /* make sure we are truly inactive */ + stop(); + + /* clear command queue */ + if (_cmd_queue != nullptr) { + delete _cmd_queue; + } +} + +int +OREOLED::init() +{ + int ret; + + /* initialise I2C bus */ + ret = I2C::init(); + if (ret != OK) { + return ret; + } + + /* allocate command queue */ + _cmd_queue = new RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); + if (_cmd_queue == nullptr) { + return ENOTTY; + } else { + /* start work queue */ + start(); + } + + return OK; +} + +int +OREOLED::probe() +{ + /* always return true */ + return OK; +} + +int +OREOLED::info() +{ + /* print health info on each LED */ + for (uint8_t i=0; icycle(); + } +} + +void +OREOLED::cycle() +{ + /* check time since startup */ + uint64_t now = hrt_absolute_time(); + bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_MS); + + /* if not leds found during start-up period, exit without rescheduling */ + if (startup_timeout && _num_healthy == 0) { + warnx("did not find oreoled"); + return; + } + + /* during startup period keep searching for unhealthy LEDs */ + if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) { + /* prepare command to turn off LED*/ + uint8_t msg[] = {OREOLED_PATTERN_OFF}; + /* attempt to contact each unhealthy LED */ + for (uint8_t i=0; iget(&next_cmd,sizeof(oreoled_cmd_t))) { + /* send valid messages to healthy LEDs */ + if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { + /* set I2C address */ + set_address(OREOLED_BASE_I2C_ADDR+next_cmd.led_num); + /* send I2C command */ + transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0); + } + } + + /* send general call every 4 seconds*/ + if ((now - _last_gencall) > OREOLED_GENERALCALL_US) { + send_general_call(); + } + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, + USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); +} + +int +OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -ENODEV; + oreoled_cmd_t new_cmd; + + switch (cmd) { + case OREOLED_SET_RGB: + /* set the specified color */ + new_cmd.led_num = ((oreoled_rgbset_t *) arg)->instance; + new_cmd.buff[0] = ((oreoled_rgbset_t *) arg)->pattern; + new_cmd.buff[1] = OREOLED_PARAM_BIAS_RED; + new_cmd.buff[2] = ((oreoled_rgbset_t *) arg)->red; + new_cmd.buff[3] = OREOLED_PARAM_BIAS_GREEN; + new_cmd.buff[4] = ((oreoled_rgbset_t *) arg)->green; + new_cmd.buff[5] = OREOLED_PARAM_BIAS_BLUE; + new_cmd.buff[6] = ((oreoled_rgbset_t *) arg)->blue; + new_cmd.num_bytes = 7; + + /* special handling for request to set all instances rgb values */ + if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { + for (uint8_t i=0; iforce(&new_cmd); + ret = OK; + } + } + + /* request to set individual instance's rgb value */ + } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { + if (_healthy[new_cmd.led_num]) { + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + return ret; + + case OREOLED_RUN_MACRO: + /* run a macro */ + new_cmd.led_num = ((oreoled_macrorun_t *) arg)->instance; + new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + new_cmd.buff[1] = OREOLED_PARAM_MACRO; + new_cmd.buff[2] = ((oreoled_macrorun_t *) arg)->macro; + new_cmd.num_bytes = 3; + + /* special handling for request to set all instances */ + if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { + for (uint8_t i=0; iforce(&new_cmd); + ret = OK; + } + } + + /* request to set individual instance's rgb value */ + } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { + if (_healthy[new_cmd.led_num]) { + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + return ret; + + default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + +/* send general call on I2C bus to syncronise all LEDs */ +int +OREOLED::send_general_call() +{ + int ret = -ENODEV; + + /* set I2C address to zero */ + set_address(0); + + /* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/ + uint8_t msg[] = {0x01,0x00}; + + /* send I2C command */ + if (transfer(msg, sizeof(msg), nullptr, 0) == OK) { + ret = OK; + } + + /* record time */ + _last_gencall = hrt_absolute_time(); + + return ret; +} + +/* send a cmd to an LEDs (used for testing only) */ +int +OREOLED::send_cmd(oreoled_cmd_t new_cmd) +{ + int ret = -ENODEV; + + /* sanity check led number, health and cmd length */ + if ((new_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[new_cmd.led_num] && (new_cmd.num_bytes < OREOLED_CMD_LENGTH_MAX)) { + /* set I2C address */ + set_address(OREOLED_BASE_I2C_ADDR+new_cmd.led_num); + + /* add to queue */ + _cmd_queue->force(&new_cmd); + ret = OK; + } + + return ret; +} + +void +oreoled_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50' 'macro 4' 'gencall' 'bytes 7 9 6'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + warnx(" -a addr (0x%x)", OREOLED_BASE_I2C_ADDR); +} + +int +oreoled_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int i2c_addr = OREOLED_BASE_I2C_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': + i2c_addr = (int)strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = (int)strtol(optarg, NULL, 0); + break; + + default: + oreoled_usage(); + exit(0); + } + } + + if (optind >= argc) { + oreoled_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int ret; + + /* start driver */ + if (!strcmp(verb, "start")) { + if (g_oreoled != nullptr) { + errx(1, "already started"); + } + + /* by default use LED bus */ + if (i2cdevice == -1) { + i2cdevice = PX4_I2C_BUS_LED; + } + + /* instantiate driver */ + g_oreoled = new OREOLED(i2cdevice, i2c_addr); + + /* check if object was created */ + if (g_oreoled == nullptr) { + errx(1, "failed to allocated memory for driver"); + } + + /* check object was created successfully */ + if (g_oreoled->init() != OK) { + delete g_oreoled; + g_oreoled = nullptr; + errx(1, "failed to start driver"); + } + + exit(0); + } + + /* need the driver past this point */ + if (g_oreoled == nullptr) { + warnx("not started"); + oreoled_usage(); + exit(1); + } + + if (!strcmp(verb, "test")) { + int fd = open(OREOLED0_DEVICE_PATH, O_RDWR); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + /* structure to hold desired colour */ + oreoled_rgbset_t rgb_set_red = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0xFF, 0x0, 0x0}; + oreoled_rgbset_t rgb_set_blue = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0x0, 0x0, 0xFF}; + oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0}; + + /* flash red and blue for 3 seconds */ + for (uint8_t i=0; i<30; i++) { + /* red */ + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_red)) != OK) { + errx(1," failed to update rgb"); + } + /* sleep for 0.05 seconds */ + usleep(50000); + /* blue */ + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_blue)) != OK) { + errx(1," failed to update rgb"); + } + /* sleep for 0.05 seconds */ + usleep(50000); + } + /* turn off LED */ + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off)) != OK) { + errx(1," failed to turn off led"); + } + + close(fd); + exit(ret); + } + + /* display driver status */ + if (!strcmp(verb, "info")) { + g_oreoled->info(); + exit(0); + } + + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + /* turn off LED */ + oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0}; + ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off); + + close(fd); + /* delete the oreoled object if stop was requested, in addition to turning off the LED. */ + if (!strcmp(verb, "stop")) { + OREOLED *tmp_oreoled = g_oreoled; + g_oreoled = nullptr; + delete tmp_oreoled; + exit(0); + } + exit(ret); + } + + /* send rgb request to all LEDS */ + if (!strcmp(verb, "rgb")) { + if (argc < 5) { + errx(1, "Usage: oreoled rgb "); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + uint8_t red = (uint8_t)strtol(argv[2], NULL, 0); + uint8_t green = (uint8_t)strtol(argv[3], NULL, 0); + uint8_t blue = (uint8_t)strtol(argv[4], NULL, 0); + oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, blue}; + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set)) != OK) { + errx(1, "failed to set rgb"); + } + + close(fd); + exit(ret); + } + + /* send macro request to all LEDS */ + if (!strcmp(verb, "macro")) { + if (argc < 3) { + errx(1, "Usage: oreoled macro "); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + uint8_t macro = (uint8_t)strtol(argv[2], NULL, 0); + + /* sanity check macro number */ + if (macro > OREOLED_PARAM_MACRO_ENUM_COUNT) { + errx(1, "invalid macro number %d",(int)macro); + exit(ret); + } + + oreoled_macrorun_t macro_run = {OREOLED_ALL_INSTANCES, (enum oreoled_macro)macro}; + if ((ret = ioctl(fd, OREOLED_RUN_MACRO, (unsigned long)¯o_run)) != OK) { + errx(1, "failed to run macro"); + } + close(fd); + exit(ret); + } + + /* send general hardware call to all LEDS */ + if (!strcmp(verb, "gencall")) { + ret = g_oreoled->send_general_call(); + warnx("sent general call"); + exit(ret); + } + + /* send a string of bytes to an LED using send_bytes function */ + if (!strcmp(verb, "bytes")) { + if (argc < 3) { + errx(1, "Usage: oreoled bytes ..."); + } + + /* structure to be sent */ + oreoled_cmd_t sendb; + + /* maximum of 20 bytes can be sent */ + if (argc > 20+3) { + errx(1, "Max of 20 bytes can be sent"); + } + + /* check led num */ + sendb.led_num = (uint8_t)strtol(argv[2], NULL, 0); + if (sendb.led_num > 3) { + errx(1, "led number must be between 0 ~ 3"); + } + + /* get bytes */ + sendb.num_bytes = argc-3; + uint8_t byte_count; + for (byte_count=0; byte_countsend_cmd(sendb)) != OK) { + errx(1, "failed to send command"); + } else { + warnx("sent %d bytes",(int)sendb.num_bytes); + } + exit(ret); + } + + oreoled_usage(); + exit(0); +} From 0616820412550c0fdd80b8cc85fd7ca45e598cac Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 25 Feb 2015 21:46:31 +0900 Subject: [PATCH 077/293] OreoLED: fix formatting --- src/drivers/oreoled/oreoled.cpp | 74 ++++++++++++++++++++++----------- 1 file changed, 49 insertions(+), 25 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index afd20275a90f..2f5bf75bc2a3 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -170,14 +170,17 @@ OREOLED::init() /* initialise I2C bus */ ret = I2C::init(); + if (ret != OK) { return ret; } /* allocate command queue */ _cmd_queue = new RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); + if (_cmd_queue == nullptr) { return ENOTTY; + } else { /* start work queue */ start(); @@ -197,11 +200,12 @@ int OREOLED::info() { /* print health info on each LED */ - for (uint8_t i=0; iget(&next_cmd,sizeof(oreoled_cmd_t))) { + + while (_cmd_queue->get(&next_cmd, sizeof(oreoled_cmd_t))) { /* send valid messages to healthy LEDs */ - if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { + if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] + && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { /* set I2C address */ - set_address(OREOLED_BASE_I2C_ADDR+next_cmd.led_num); + set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); /* send I2C command */ transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0); } @@ -311,7 +320,7 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) /* special handling for request to set all instances rgb values */ if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { - for (uint8_t i=0; iforce(&new_cmd); ret = OK; } } + return ret; case OREOLED_RUN_MACRO: @@ -339,7 +349,7 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) /* special handling for request to set all instances */ if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { - for (uint8_t i=0; iforce(&new_cmd); ret = OK; } } + return ret; default: @@ -376,7 +387,7 @@ OREOLED::send_general_call() set_address(0); /* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/ - uint8_t msg[] = {0x01,0x00}; + uint8_t msg[] = {0x01, 0x00}; /* send I2C command */ if (transfer(msg, sizeof(msg), nullptr, 0) == OK) { @@ -398,7 +409,7 @@ OREOLED::send_cmd(oreoled_cmd_t new_cmd) /* sanity check led number, health and cmd length */ if ((new_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[new_cmd.led_num] && (new_cmd.num_bytes < OREOLED_CMD_LENGTH_MAX)) { /* set I2C address */ - set_address(OREOLED_BASE_I2C_ADDR+new_cmd.led_num); + set_address(OREOLED_BASE_I2C_ADDR + new_cmd.led_num); /* add to queue */ _cmd_queue->force(&new_cmd); @@ -500,23 +511,27 @@ oreoled_main(int argc, char *argv[]) oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0}; /* flash red and blue for 3 seconds */ - for (uint8_t i=0; i<30; i++) { + for (uint8_t i = 0; i < 30; i++) { /* red */ if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_red)) != OK) { - errx(1," failed to update rgb"); + errx(1, " failed to update rgb"); } + /* sleep for 0.05 seconds */ usleep(50000); + /* blue */ if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_blue)) != OK) { - errx(1," failed to update rgb"); + errx(1, " failed to update rgb"); } + /* sleep for 0.05 seconds */ usleep(50000); } + /* turn off LED */ if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off)) != OK) { - errx(1," failed to turn off led"); + errx(1, " failed to turn off led"); } close(fd); @@ -541,6 +556,7 @@ oreoled_main(int argc, char *argv[]) ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off); close(fd); + /* delete the oreoled object if stop was requested, in addition to turning off the LED. */ if (!strcmp(verb, "stop")) { OREOLED *tmp_oreoled = g_oreoled; @@ -548,6 +564,7 @@ oreoled_main(int argc, char *argv[]) delete tmp_oreoled; exit(0); } + exit(ret); } @@ -567,6 +584,7 @@ oreoled_main(int argc, char *argv[]) uint8_t green = (uint8_t)strtol(argv[3], NULL, 0); uint8_t blue = (uint8_t)strtol(argv[4], NULL, 0); oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, blue}; + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set)) != OK) { errx(1, "failed to set rgb"); } @@ -591,14 +609,16 @@ oreoled_main(int argc, char *argv[]) /* sanity check macro number */ if (macro > OREOLED_PARAM_MACRO_ENUM_COUNT) { - errx(1, "invalid macro number %d",(int)macro); + errx(1, "invalid macro number %d", (int)macro); exit(ret); } oreoled_macrorun_t macro_run = {OREOLED_ALL_INSTANCES, (enum oreoled_macro)macro}; + if ((ret = ioctl(fd, OREOLED_RUN_MACRO, (unsigned long)¯o_run)) != OK) { errx(1, "failed to run macro"); } + close(fd); exit(ret); } @@ -620,29 +640,33 @@ oreoled_main(int argc, char *argv[]) oreoled_cmd_t sendb; /* maximum of 20 bytes can be sent */ - if (argc > 20+3) { + if (argc > 20 + 3) { errx(1, "Max of 20 bytes can be sent"); } /* check led num */ sendb.led_num = (uint8_t)strtol(argv[2], NULL, 0); + if (sendb.led_num > 3) { errx(1, "led number must be between 0 ~ 3"); } /* get bytes */ - sendb.num_bytes = argc-3; + sendb.num_bytes = argc - 3; uint8_t byte_count; - for (byte_count=0; byte_countsend_cmd(sendb)) != OK) { errx(1, "failed to send command"); + } else { - warnx("sent %d bytes",(int)sendb.num_bytes); + warnx("sent %d bytes", (int)sendb.num_bytes); } + exit(ret); } From cc73e30f3b4f0cf9f634093bf10a63968ad4719d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 25 Feb 2015 21:55:22 +0900 Subject: [PATCH 078/293] build: add oreoled to px4fmu-v2_default --- makefiles/config_px4fmu-v2_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 524786f7805e..4f56f14e213a 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -43,6 +43,7 @@ MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl MODULES += drivers/px4flow +MODULES += drivers/oreoled # # System commands From 9ff269dba16e9bc0dce15efb6641fb68c0b9bbf1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 2 Mar 2015 14:30:46 +0900 Subject: [PATCH 079/293] batt_smbus: remove debug --- src/drivers/batt_smbus/batt_smbus.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 94ddfd34d689..222d26d74c69 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -452,10 +452,7 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_ uint8_t pec = get_PEC(reg, true, buff, bufflen + 1); if (pec != buff[bufflen + 1]) { - // debug - warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec); return 0; - } // copy data From 531bc4fddb3669cc4cd2acad771c455cab698f6a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 2 Mar 2015 14:30:01 +0900 Subject: [PATCH 080/293] oreoled: support send_bytes ioctl Also increase maximum command length to 24 bytes --- src/drivers/drv_oreoled.h | 5 ++++- src/drivers/oreoled/oreoled.cpp | 25 +++++++++++++++++++++++++ 2 files changed, 29 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index bc53f04a541e..0dcb10a7b1ef 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -58,6 +58,9 @@ /** run macro */ #define OREOLED_RUN_MACRO _OREOLEDIOC(2) +/** send bytes */ +#define OREOLED_SEND_BYTES _OREOLEDIOC(3) + /* Oreo LED driver supports up to 4 leds */ #define OREOLED_NUM_LEDS 4 @@ -65,7 +68,7 @@ #define OREOLED_ALL_INSTANCES 0xff /* maximum command length that can be sent to LEDs */ -#define OREOLED_CMD_LENGTH_MAX 10 +#define OREOLED_CMD_LENGTH_MAX 24 /* enum passed to OREOLED_SET_MODE ioctl() * defined by hardware */ diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 2f5bf75bc2a3..b44c4b720ba4 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -368,6 +368,31 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; + case OREOLED_SEND_BYTES: + /* send bytes */ + new_cmd = *((oreoled_cmd_t *) arg); + + /* special handling for request to set all instances */ + if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + /* add command to queue for all healthy leds */ + if (_healthy[i]) { + new_cmd.led_num = i; + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { + /* request to set individual instance's rgb value */ + if (_healthy[new_cmd.led_num]) { + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + return ret; + default: /* see if the parent class can make any use of it */ ret = CDev::ioctl(filp, cmd, arg); From 5db1af50b7e03f9036e311fd6acdb6220b4d3cd5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Mar 2015 14:46:44 +1100 Subject: [PATCH 081/293] px_loader: added --force option this can be used to override the board type check. Useful when changing bootloaders --- Tools/px_uploader.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 95a3d4046960..ddc9fdabea8c 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -416,7 +416,12 @@ def identify(self): def upload(self, fw): # Make sure we are doing the right thing if self.board_type != fw.property('board_id'): - raise RuntimeError("Firmware not suitable for this board") + msg = "Firmware not suitable for this board (board_type=%u board_id=%u)" % ( + self.board_type, fw.property('board_id')) + if args.force: + print("WARNING: %s" % msg) + else: + raise RuntimeError(msg) if self.fw_maxsize < fw.property('image_size'): raise RuntimeError("Firmware image is too large for this board") @@ -486,6 +491,7 @@ def send_reboot(self): parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") +parser.add_argument('--force', action='store_true', default=False, help='Override board type check and continue loading') parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") args = parser.parse_args() From 2415a9eaaf27f056bb03e987318992a02acfec89 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Mar 2015 22:00:40 +1100 Subject: [PATCH 082/293] px_uploader: print chip version --- Tools/px_uploader.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index ddc9fdabea8c..65a7e1a5e4a0 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -160,6 +160,7 @@ class uploader(object): GET_CRC = b'\x29' # rev3+ GET_OTP = b'\x2a' # rev4+ , get a word from OTP area GET_SN = b'\x2b' # rev4+ , get a word from SN area + GET_CHIP = b'\x2c' # rev5+ , get chip version REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision @@ -258,7 +259,7 @@ def __getOTP(self, param): self.__getSync() return value - # send the GET_OTP command and wait for an info parameter + # send the GET_SN command and wait for an info parameter def __getSN(self, param): t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. self.__send(uploader.GET_SN + t + uploader.EOC) @@ -266,6 +267,13 @@ def __getSN(self, param): self.__getSync() return value + # send the GET_CHIP command + def __getCHIP(self): + self.__send(uploader.GET_CHIP + uploader.EOC) + value = self.__recv_int() + self.__getSync() + return value + def __drawProgressBar(self, label, progress, maxVal): if maxVal < progress: progress = maxVal @@ -451,6 +459,7 @@ def upload(self, fw): self.sn = self.sn + x print(binascii.hexlify(x).decode('Latin-1'), end='') # show user print('') + print("chip: %08x" % self.__getCHIP()) except Exception: # ignore bad character encodings pass From c3ce144dbaca2f30a4ca16f356bace4290c306a9 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 16 Feb 2015 18:19:17 -0800 Subject: [PATCH 083/293] mpu6000: add set_accel_range --- src/drivers/mpu6000/mpu6000.cpp | 111 ++++++++++++-------------------- 1 file changed, 40 insertions(+), 71 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 4aa05a980818..239eb4d930a1 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -359,7 +359,7 @@ class MPU6000 : public device::SPI * @param max_g The maximum G value the range must support. * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_range(unsigned max_g); + int set_accel_range(unsigned max_g); /** * Swap a 16-bit value read from the MPU6000 to native byte order. @@ -713,37 +713,7 @@ int MPU6000::reset() _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; - // product-specific scaling - switch (_product) { - case MPU6000ES_REV_C4: - case MPU6000ES_REV_C5: - case MPU6000_REV_C4: - case MPU6000_REV_C5: - // Accel scale 8g (4096 LSB/g) - // Rev C has different scaling than rev D - write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); - break; - - case MPU6000ES_REV_D6: - case MPU6000ES_REV_D7: - case MPU6000ES_REV_D8: - case MPU6000_REV_D6: - case MPU6000_REV_D7: - case MPU6000_REV_D8: - case MPU6000_REV_D9: - case MPU6000_REV_D10: - // default case to cope with new chip revisions, which - // presumably won't have the accel scaling bug - default: - // Accel scale 8g (4096 LSB/g) - write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); - break; - } - - // Correct accel scale factors of 4096 LSB/g - // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_range_scale = (MPU6000_ONE_G / 4096.0f); - _accel_range_m_s2 = 8.0f * MPU6000_ONE_G; + set_accel_range(8); usleep(1000); @@ -1297,11 +1267,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case ACCELIOCSRANGE: - /* XXX not implemented */ - // XXX change these two values on set: - // _accel_range_scale = (9.81f / 4096.0f); - // _accel_range_m_s2 = 8.0f * 9.81f; - return -EINVAL; + return set_accel_range(arg); + case ACCELIOCGRANGE: return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f); @@ -1451,44 +1418,46 @@ MPU6000::write_checked_reg(unsigned reg, uint8_t value) } int -MPU6000::set_range(unsigned max_g) +MPU6000::set_accel_range(unsigned max_g_in) { -#if 0 - uint8_t rangebits; - float rangescale; - - if (max_g > 16) { - return -ERANGE; - - } else if (max_g > 8) { /* 16G */ - rangebits = OFFSET_LSB1_RANGE_16G; - rangescale = 1.98; - - } else if (max_g > 4) { /* 8G */ - rangebits = OFFSET_LSB1_RANGE_8G; - rangescale = 0.99; - - } else if (max_g > 3) { /* 4G */ - rangebits = OFFSET_LSB1_RANGE_4G; - rangescale = 0.5; - - } else if (max_g > 2) { /* 3G */ - rangebits = OFFSET_LSB1_RANGE_3G; - rangescale = 0.38; - - } else if (max_g > 1) { /* 2G */ - rangebits = OFFSET_LSB1_RANGE_2G; - rangescale = 0.25; + // workaround for bugged versions of MPU6k (rev C) + switch (_product) { + case MPU6000ES_REV_C4: + case MPU6000ES_REV_C5: + case MPU6000_REV_C4: + case MPU6000_REV_C5: + write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); + _accel_range_scale = (MPU6000_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * MPU6000_ONE_G; + return OK; + } - } else { /* 1G */ - rangebits = OFFSET_LSB1_RANGE_1G; - rangescale = 0.13; + uint8_t afs_sel; + float lsb_per_g; + float max_accel_g; + + if (max_g_in > 8) { // 16g - AFS_SEL = 3 + afs_sel = 3; + lsb_per_g = 2048; + max_accel_g = 16; + } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 + afs_sel = 2; + lsb_per_g = 4096; + max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 + afs_sel = 1; + lsb_per_g = 8192; + max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 + afs_sel = 0; + lsb_per_g = 16384; + max_accel_g = 2; } - /* adjust sensor configuration */ - modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits); - _range_scale = rangescale; -#endif + write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); + _accel_range_scale = (MPU6000_ONE_G / lsb_per_g); + _accel_range_m_s2 = max_accel_g * MPU6000_ONE_G; + return OK; } From 675a5e407d571d9eb81d062e22d58e0e4a7bef5f Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 17 Feb 2015 03:49:40 -0800 Subject: [PATCH 084/293] drv_accel: add ACCELIOCSHWLOWPASS to set hardware lowpass --- src/drivers/drv_accel.h | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index fccd446ad04c..88c8a35933da 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -105,10 +105,10 @@ ORB_DECLARE(sensor_accel); /** return the accel internal sample rate in Hz */ #define ACCELIOCGSAMPLERATE _ACCELIOC(1) -/** set the accel internal lowpass filter to no lower than (arg) Hz */ +/** set the software low-pass filter cut-off in Hz */ #define ACCELIOCSLOWPASS _ACCELIOC(2) -/** return the accel internal lowpass filter in Hz */ +/** get the software low-pass filter cut-off in Hz */ #define ACCELIOCGLOWPASS _ACCELIOC(3) /** set the accel scaling constants to the structure pointed to by (arg) */ @@ -126,4 +126,10 @@ ORB_DECLARE(sensor_accel); /** get the result of a sensor self-test */ #define ACCELIOCSELFTEST _ACCELIOC(9) +/** set the hardware low-pass filter cut-off no lower than (arg) Hz */ +#define ACCELIOCSHWLOWPASS _ACCELIOC(10) + +/** get the hardware low-pass filter cut-off in Hz*/ +#define ACCELIOCGHWLOWPASS _ACCELIOC(11) + #endif /* _DRV_ACCEL_H */ From 22b975cd6ad9fb9b41c4d7838be840f77ac06d84 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 17 Feb 2015 03:50:19 -0800 Subject: [PATCH 085/293] drv_gyro: add GYROIOCSHWLOWPASS to set hardware lowpass --- src/drivers/drv_gyro.h | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 122d204159f5..ec828a5eab24 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -101,10 +101,10 @@ ORB_DECLARE(sensor_gyro); /** return the gyro internal sample rate in Hz */ #define GYROIOCGSAMPLERATE _GYROIOC(1) -/** set the gyro internal lowpass filter to no lower than (arg) Hz */ +/** set the software low-pass filter cut-off in Hz */ #define GYROIOCSLOWPASS _GYROIOC(2) -/** set the gyro internal lowpass filter to no lower than (arg) Hz */ +/** get the software low-pass filter cut-off in Hz */ #define GYROIOCGLOWPASS _GYROIOC(3) /** set the gyro scaling constants to (arg) */ @@ -122,4 +122,10 @@ ORB_DECLARE(sensor_gyro); /** check the status of the sensor */ #define GYROIOCSELFTEST _GYROIOC(8) +/** set the hardware low-pass filter cut-off no lower than (arg) Hz */ +#define GYROIOCSHWLOWPASS _GYROIOC(9) + +/** get the hardware low-pass filter cut-off in Hz*/ +#define GYROIOCGHWLOWPASS _GYROIOC(10) + #endif /* _DRV_GYRO_H */ From e5f7780bfcffbdf481e9dd5026695704d580a044 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 17 Feb 2015 03:51:04 -0800 Subject: [PATCH 086/293] l3gd20: add support for GYROIOCSHWLOWPASS --- src/drivers/l3gd20/l3gd20.cpp | 53 +++++++++++++++++++++++++++++------ 1 file changed, 45 insertions(+), 8 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index ad01338b9fa1..e577602a6d30 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -231,6 +231,7 @@ class L3GD20 : public device::SPI int _class_instance; unsigned _current_rate; + unsigned _current_bandwidth; unsigned _orientation; unsigned _read; @@ -360,7 +361,7 @@ class L3GD20 : public device::SPI * Zero selects the maximum rate supported. * @return OK if the value can be supported. */ - int set_samplerate(unsigned frequency); + int set_samplerate(unsigned frequency, unsigned bandwidth); /** * Set the lowpass filter of the driver @@ -407,6 +408,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _orb_class_instance(-1), _class_instance(-1), _current_rate(0), + _current_bandwidth(50), _orientation(SENSOR_BOARD_ROTATION_DEFAULT), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), @@ -653,12 +655,13 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case GYROIOCSSAMPLERATE: - return set_samplerate(arg); + return set_samplerate(arg, _current_bandwidth); case GYROIOCGSAMPLERATE: return _current_rate; case GYROIOCSLOWPASS: { + // set the software lowpass cut-off in Hz float cutoff_freq_hz = arg; float sample_rate = 1.0e6f / _call_interval; set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); @@ -690,6 +693,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSELFTEST: return self_test(); + case GYROIOCSHWLOWPASS: + return set_samplerate(_current_rate, arg); + + case GYROIOCGHWLOWPASS: + return _current_bandwidth; + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -780,7 +789,7 @@ L3GD20::set_range(unsigned max_dps) } int -L3GD20::set_samplerate(unsigned frequency) +L3GD20::set_samplerate(unsigned frequency, unsigned bandwidth) { uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; @@ -794,19 +803,47 @@ L3GD20::set_samplerate(unsigned frequency) */ if (frequency <= 100) { _current_rate = _is_l3g4200d ? 100 : 95; + _current_bandwidth = 25; bits |= RATE_95HZ_LP_25HZ; - } else if (frequency <= 200) { _current_rate = _is_l3g4200d ? 200 : 190; - bits |= RATE_190HZ_LP_50HZ; + if (bandwidth <= 25) { + _current_bandwidth = 25; + bits |= RATE_190HZ_LP_25HZ; + } else if (bandwidth <= 50) { + _current_bandwidth = 50; + bits |= RATE_190HZ_LP_50HZ; + } else { + _current_bandwidth = 70; + bits |= RATE_190HZ_LP_70HZ; + } } else if (frequency <= 400) { _current_rate = _is_l3g4200d ? 400 : 380; - bits |= RATE_380HZ_LP_50HZ; + if (bandwidth <= 25) { + _current_bandwidth = 25; + bits |= RATE_380HZ_LP_25HZ; + } else if (bandwidth <= 50) { + _current_bandwidth = 50; + bits |= RATE_380HZ_LP_50HZ; + } else { + _current_bandwidth = _is_l3g4200d ? 110 : 100; + bits |= RATE_380HZ_LP_100HZ; + } } else if (frequency <= 800) { _current_rate = _is_l3g4200d ? 800 : 760; - bits |= RATE_760HZ_LP_50HZ; + if (bandwidth <= 30) { + _current_bandwidth = 30; + bits |= RATE_760HZ_LP_30HZ; + } else if (bandwidth <= 50) { + _current_bandwidth = 50; + bits |= RATE_760HZ_LP_50HZ; + } else { + _current_bandwidth = _is_l3g4200d ? 110 : 100; + bits |= RATE_760HZ_LP_100HZ; + } + } else { return -EINVAL; } @@ -883,7 +920,7 @@ L3GD20::reset() * callback fast enough to not miss data. */ write_checked_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - set_samplerate(0); // 760Hz or 800Hz + set_samplerate(0, _current_bandwidth); // 760Hz or 800Hz set_range(L3GD20_DEFAULT_RANGE_DPS); set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); From 5df711aa9c07cf6d26202b203601b6809f8ddcf3 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 17 Feb 2015 03:52:15 -0800 Subject: [PATCH 087/293] lsm303d: support ACCELIOCSHWLOWPASS --- src/drivers/lsm303d/lsm303d.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index f42b31cdfbfd..34a5fc117759 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -950,6 +950,13 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSELFTEST: return accel_self_test(); + case ACCELIOCSHWLOWPASS: + accel_set_onchip_lowpass_filter_bandwidth(arg); + return OK; + + case ACCELIOCGHWLOWPASS: + return _accel_onchip_filter_bandwith; + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); From 2b72fedf10f4e04065ee9eb2f2185168d65f6f1d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 17 Feb 2015 03:52:53 -0800 Subject: [PATCH 088/293] mpu6k: add support for ACCELIOCSHWLOWPASS and GYROIOCSHWLOWPASS --- src/drivers/mpu6000/mpu6000.cpp | 37 +++++++++++++++++++++++++++------ 1 file changed, 31 insertions(+), 6 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 239eb4d930a1..b1043c2e17cd 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -249,6 +249,8 @@ class MPU6000 : public device::SPI float _gyro_range_scale; float _gyro_range_rad_s; + unsigned _dlpf_freq; + unsigned _sample_rate; perf_counter_t _accel_reads; perf_counter_t _gyro_reads; @@ -498,6 +500,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), + _dlpf_freq(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ), _sample_rate(1000), _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), @@ -790,23 +793,32 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) /* choose next highest filter frequency available */ - if (frequency_hz == 0) { + if (frequency_hz == 0) { + _dlpf_freq = 0; filter = BITS_DLPF_CFG_2100HZ_NOLPF; - } else if (frequency_hz <= 5) { + } else if (frequency_hz <= 5) { + _dlpf_freq = 5; filter = BITS_DLPF_CFG_5HZ; } else if (frequency_hz <= 10) { + _dlpf_freq = 10; filter = BITS_DLPF_CFG_10HZ; } else if (frequency_hz <= 20) { + _dlpf_freq = 20; filter = BITS_DLPF_CFG_20HZ; } else if (frequency_hz <= 42) { + _dlpf_freq = 42; filter = BITS_DLPF_CFG_42HZ; } else if (frequency_hz <= 98) { + _dlpf_freq = 98; filter = BITS_DLPF_CFG_98HZ; } else if (frequency_hz <= 188) { + _dlpf_freq = 188; filter = BITS_DLPF_CFG_188HZ; } else if (frequency_hz <= 256) { + _dlpf_freq = 256; filter = BITS_DLPF_CFG_256HZ_NOLPF2; } else { + _dlpf_freq = 0; filter = BITS_DLPF_CFG_2100HZ_NOLPF; } write_checked_reg(MPUREG_CONFIG, filter); @@ -1240,8 +1252,6 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSLOWPASS: - // set hardware filtering - _set_dlpf_filter(arg); // set software filtering _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); @@ -1275,6 +1285,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSELFTEST: return accel_self_test(); + case ACCELIOCSHWLOWPASS: + _set_dlpf_filter(arg); + return OK; + + case ACCELIOCGHWLOWPASS: + return _dlpf_freq; + + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -1319,9 +1337,9 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); + case GYROIOCSLOWPASS: - // set hardware filtering - _set_dlpf_filter(arg); + // set software filtering _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); @@ -1349,6 +1367,13 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSELFTEST: return gyro_self_test(); + case GYROIOCSHWLOWPASS: + _set_dlpf_filter(arg); + return OK; + + case GYROIOCGHWLOWPASS: + return _dlpf_freq; + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); From 46aed37f6fc251699009a4d3c563d661258684de Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Mar 2015 13:56:28 +1100 Subject: [PATCH 089/293] FMUv2: added bootloader delay signature to text this allows for a configurable bootloader delay --- nuttx-configs/px4fmu-v2/scripts/ld.script | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script index bec896d1ce53..b04ad89a6c6c 100644 --- a/nuttx-configs/px4fmu-v2/scripts/ld.script +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -66,12 +66,20 @@ EXTERN(_vectors) /* force the vectors to be included in the output */ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). */ EXTERN(abort) +EXTERN(_bootdelay_signature) SECTIONS { .text : { _stext = ABSOLUTE(.); *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; *(.text .text.*) *(.fixup) *(.gnu.warning) From 8ad2ffca171ec2bd4bbaf7d84caa29a628723d12 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Mar 2015 13:56:54 +1100 Subject: [PATCH 090/293] px_uploader.py: added --boot-delay option this sets the bootloader delay --- Tools/px_uploader.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 65a7e1a5e4a0..9950f99716e6 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -161,6 +161,7 @@ class uploader(object): GET_OTP = b'\x2a' # rev4+ , get a word from OTP area GET_SN = b'\x2b' # rev4+ , get a word from SN area GET_CHIP = b'\x2c' # rev5+ , get chip version + SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision @@ -405,6 +406,12 @@ def __verify_v3(self, label, fw): raise RuntimeError("Program CRC failed") self.__drawProgressBar(label, 100, 100) + def __set_boot_delay(self, boot_delay): + self.__send(uploader.SET_BOOT_DELAY + + struct.pack("b", boot_delay) + + uploader.EOC) + self.__getSync() + # get basic data about the board def identify(self): # make sure we are in sync before starting @@ -472,6 +479,9 @@ def upload(self, fw): else: self.__verify_v3("Verify ", fw) + if args.boot_delay is not None: + self.__set_boot_delay(args.boot_delay) + print("\nRebooting.\n") self.__reboot() self.port.close() @@ -501,6 +511,7 @@ def send_reboot(self): parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") parser.add_argument('--force', action='store_true', default=False, help='Override board type check and continue loading') +parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") args = parser.parse_args() From 0c9d7c1263fa216a7f5c7bd745b3014900d20d1d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Mar 2015 14:17:21 +1100 Subject: [PATCH 091/293] Tools: added boot_now.py script --- Tools/boot_now.py | 59 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100755 Tools/boot_now.py diff --git a/Tools/boot_now.py b/Tools/boot_now.py new file mode 100755 index 000000000000..5a9e608dad96 --- /dev/null +++ b/Tools/boot_now.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# 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. +# +############################################################################ + +# send BOOT command to a device + +import argparse +import serial, sys + +from sys import platform as _platform + +# Parse commandline arguments +parser = argparse.ArgumentParser(description="Send boot command to a device") +parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port") +parser.add_argument('port', action="store", help="Serial port(s) to which the FMU may be attached") +args = parser.parse_args() + +REBOOT = b'\x30' +EOC = b'\x20' + +print("Sending reboot to %s" % args.port) +try: + port = serial.Serial(args.port, args.baud, timeout=0.5) +except Exception: + print("Unable to open %s" % args.port) + sys.exit(1) +port.write(REBOOT + EOC) +port.close() +sys.exit(0) From d67068f77d3727095934e13cf70ee6dea7294004 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Fri, 28 Nov 2014 11:31:01 +0100 Subject: [PATCH 092/293] uavcan-baro: add read()-style interface to baro device --- src/modules/uavcan/sensors/baro.cpp | 44 ++++++++++++++++++++++++----- src/modules/uavcan/sensors/baro.hpp | 2 ++ 2 files changed, 39 insertions(+), 7 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 5348d4418004..26a5fb27e1da 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -61,6 +61,30 @@ int UavcanBarometerBridge::init() return 0; } +ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen) +{ + static uint64_t last_read = 0; + struct baro_report *baro_buf = reinterpret_cast(buffer); + + /* buffer must be large enough */ + unsigned count = buflen / sizeof(struct baro_report); + if (count < 1) { + return -ENOSPC; + } + + if (last_read < _report.timestamp) { + /* copy report */ + lock(); + *baro_buf = _report; + last_read = _report.timestamp; + unlock(); + return sizeof(struct baro_report); + } else { + /* no new data available, warn caller */ + return -EAGAIN; + } +} + int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -76,6 +100,14 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) case BAROIOCGMSLPRESSURE: { return _msl_pressure; } + case SENSORIOCSPOLLRATE: { + // not supported yet, pretend that everything is ok + return OK; + } + case SENSORIOCSQUEUEDEPTH: { + // not supported yet, pretend that everything is ok + return OK; + } default: { return CDev::ioctl(filp, cmd, arg); } @@ -84,11 +116,9 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) { - auto report = ::baro_report(); - - report.timestamp = msg.getMonotonicTimestamp().toUSec(); - report.temperature = msg.static_temperature; - report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + _report.timestamp = msg.getMonotonicTimestamp().toUSec(); + _report.temperature = msg.static_temperature; + _report.pressure = msg.static_pressure / 100.0F; // Convert to millibar /* * Altitude computation @@ -102,7 +132,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa - report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + _report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - publish(msg.getSrcNodeID().get(), &report); + publish(msg.getSrcNodeID().get(), &_report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index c7bbc5af8db5..f64ea0100a35 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -54,6 +54,7 @@ class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase 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 air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); @@ -65,4 +66,5 @@ class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase uavcan::Subscriber _sub_air_data; unsigned _msl_pressure = 101325; + baro_report _report = {}; }; From f62c6259f6e899debd1f73458d7ef64d7964a1af Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 Mar 2015 20:14:06 +1100 Subject: [PATCH 093/293] uavcan-baro: use RingBuffer for read() support --- src/modules/uavcan/sensors/baro.cpp | 74 ++++++++++++++++++----------- src/modules/uavcan/sensors/baro.hpp | 4 +- 2 files changed, 50 insertions(+), 28 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 26a5fb27e1da..350078b14d6f 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -36,13 +36,15 @@ */ #include "baro.hpp" +#include #include const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), -_sub_air_data(node) +_sub_air_data(node), +_reports(nullptr) { } @@ -53,6 +55,11 @@ int UavcanBarometerBridge::init() return res; } + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(baro_report)); + if (_reports == nullptr) + return -1; + res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); @@ -63,26 +70,23 @@ int UavcanBarometerBridge::init() ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen) { - static uint64_t last_read = 0; + unsigned count = buflen / sizeof(struct baro_report); struct baro_report *baro_buf = reinterpret_cast(buffer); + int ret = 0; /* buffer must be large enough */ - unsigned count = buflen / sizeof(struct baro_report); - if (count < 1) { + if (count < 1) return -ENOSPC; - } - if (last_read < _report.timestamp) { - /* copy report */ - lock(); - *baro_buf = _report; - last_read = _report.timestamp; - unlock(); - return sizeof(struct baro_report); - } else { - /* no new data available, warn caller */ - return -EAGAIN; + while (count--) { + if (_reports->get(baro_buf)) { + ret += sizeof(*baro_buf); + baro_buf++; + } } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; } int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) @@ -105,7 +109,17 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; } case SENSORIOCSQUEUEDEPTH: { - // not supported yet, pretend that everything is ok + /* 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; } default: { @@ -116,23 +130,29 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) { - _report.timestamp = msg.getMonotonicTimestamp().toUSec(); - _report.temperature = msg.static_temperature; - _report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + baro_report report; + + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + report.temperature = msg.static_temperature; + report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + report.error_count = 0; /* * Altitude computation * Refer to the MS5611 driver for details */ - const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin - const double a = -6.5 / 1000; // temperature gradient in degrees per metre - const double g = 9.80665; // gravity constant in m/s/s - const double R = 287.05; // ideal gas constant in J/kg/K + const float T1 = 15.0f + 273.15f; // temperature at base height in Kelvin + const float a = -6.5f / 1000; // temperature gradient in degrees per metre + const float g = 9.80665f; // gravity constant in m/s/s + const float R = 287.05f; // ideal gas constant in J/kg/K + + const float p1 = _msl_pressure / 1000.0f; // current pressure at MSL in kPa + const float p = msg.static_pressure / 1000.0f; // measured pressure in kPa - const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa - const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa + report.altitude = (((std::powf((p / p1), (-(a * R) / g))) * T1) - T1) / a; - _report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + // add to the ring buffer + _reports->force(&report); - publish(msg.getSrcNodeID().get(), &_report); + publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index f64ea0100a35..2363040b56cd 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -42,6 +42,8 @@ #include +class RingBuffer; + class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase { public: @@ -66,5 +68,5 @@ class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase uavcan::Subscriber _sub_air_data; unsigned _msl_pressure = 101325; - baro_report _report = {}; + RingBuffer *_reports; }; From 9c5ba2c4233ecf7c1b27e2bc34ef1ecdb2149eeb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 17 Mar 2015 13:25:02 +1100 Subject: [PATCH 094/293] lsm303d: support temperature readings in lsm303d these are read at the same rate as the mag --- src/drivers/lsm303d/lsm303d.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 34a5fc117759..1e76599b712d 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -305,6 +305,9 @@ class LSM303D : public device::SPI float _last_accel[3]; uint8_t _constant_accel_count; + // last temperature value + float _last_temperature; + // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset @@ -567,6 +570,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _constant_accel_count(0), + _last_temperature(0), _checked_next(0) { @@ -711,7 +715,7 @@ LSM303D::reset() /* enable mag */ write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T); write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 @@ -1511,6 +1515,9 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); + // use the temperature from the last mag reading + accel_report.temperature = _last_temperature; + // report the error count as the sum of the number of bad // register reads and bad values. This allows the higher level // code to decide if it should use this sensor based on @@ -1588,6 +1595,7 @@ LSM303D::mag_measure() #pragma pack(push, 1) struct { uint8_t cmd; + int16_t temperature; uint8_t status; int16_t x; int16_t y; @@ -1603,7 +1611,7 @@ LSM303D::mag_measure() /* fetch data from the sensor */ memset(&raw_mag_report, 0, sizeof(raw_mag_report)); - raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + raw_mag_report.cmd = ADDR_OUT_TEMP_L | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); /* @@ -1634,6 +1642,10 @@ LSM303D::mag_measure() mag_report.range_ga = (float)_mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); + // remember the temperature. The datasheet isn't clear, but it + // seems to be a signed offset from 25 degrees C in units of 0.125C + _last_temperature = 25 + (raw_mag_report.temperature*0.125f); + // apply user specified rotation rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z); @@ -1676,6 +1688,7 @@ LSM303D::print_info() (unsigned)_checked_values[i]); } } + ::printf("temperature: %.2f\n", _last_temperature); } void From b89470d8a645407ac7e20ac4b883856c5d2cbfc8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 17 Mar 2015 13:25:27 +1100 Subject: [PATCH 095/293] mpu6000: show temperature in "mpu6000 info" --- src/drivers/mpu6000/mpu6000.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b1043c2e17cd..7bbd717bad1e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -286,6 +286,9 @@ class MPU6000 : public device::SPI // self test volatile bool _in_factory_test; + // last temperature reading for print_info() + float _last_temperature; + /** * Start automatic measurement. */ @@ -521,7 +524,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _checked_next(0), - _in_factory_test(false) + _in_factory_test(false), + _last_temperature(0) { // disable debug() calls _debug_enabled = false; @@ -1719,8 +1723,10 @@ MPU6000::measure() arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; + _last_temperature = (report.temp) / 361.0f + 35.0f; + arb.temperature_raw = report.temp; - arb.temperature = (report.temp) / 361.0f + 35.0f; + arb.temperature = _last_temperature; grb.x_raw = report.gyro_x; grb.y_raw = report.gyro_y; @@ -1741,7 +1747,7 @@ MPU6000::measure() grb.range_rad_s = _gyro_range_rad_s; grb.temperature_raw = report.temp; - grb.temperature = (report.temp) / 361.0f + 35.0f; + grb.temperature = _last_temperature; _accel_reports->force(&arb); _gyro_reports->force(&grb); @@ -1789,6 +1795,7 @@ MPU6000::print_info() (unsigned)_checked_values[i]); } } + ::printf("temperature: %.1f\n", _last_temperature); } void From 1fae9e665308371bac3301bcb9487274287c194b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 18 Mar 2015 20:05:45 +0900 Subject: [PATCH 096/293] batt_smbus: calculate current discharged --- src/drivers/batt_smbus/batt_smbus.cpp | 25 +++++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 222d26d74c69..2d5ce1473f4f 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -84,6 +84,7 @@ #define BATT_SMBUS_ADDR 0x0B ///< I2C address #define BATT_SMBUS_TEMP 0x08 ///< temperature register #define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register +#define BATT_SMBUS_REMAINING_CAPACITY 0x0f ///< predicted remaining battery capacity as a percentage #define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register #define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register @@ -179,6 +180,7 @@ class BATT_SMBUS : public device::I2C orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID uint64_t _start_time; ///< system time we first attempt to communicate with battery + uint16_t _batt_design_capacity; ///< battery's design capacity in mAh (0 means unknown) }; namespace @@ -197,7 +199,8 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : _reports(nullptr), _batt_topic(-1), _batt_orb_id(nullptr), - _start_time(0) + _start_time(0), + _batt_design_capacity(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -263,7 +266,7 @@ BATT_SMBUS::test() if (updated) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { - warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a); + warnx("V=%4.2f C=%4.2f DismAh=%4.2f", (float)status.voltage_v, (float)status.current_a, (float)status.discharged_mah); } } @@ -371,6 +374,24 @@ BATT_SMBUS::cycle() new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | (uint32_t)buff[0])) / 1000.0f; } + // read battery design capacity + if (_batt_design_capacity == 0) { + usleep(1); + if (read_reg(BATT_SMBUS_DESIGN_CAPACITY, tmp) == OK) { + _batt_design_capacity = tmp; + } + } + + // read remaining capacity + if (_batt_design_capacity > 0) { + usleep(1); + if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { + if (tmp < _batt_design_capacity) { + new_report.discharged_mah = _batt_design_capacity - tmp; + } + } + } + // publish to orb if (_batt_topic != -1) { orb_publish(_batt_orb_id, _batt_topic, &new_report); From f5a8173bb14ad47addba82fe1850cb5a92b208f9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 19 Mar 2015 11:54:21 +0900 Subject: [PATCH 097/293] batt_smbus: remove usleep Also restore I2C address after performing command line search for devices --- src/drivers/batt_smbus/batt_smbus.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 2d5ce1473f4f..2008e0c7ce12 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -282,6 +282,7 @@ BATT_SMBUS::search() { bool found_slave = false; uint16_t tmp; + int16_t orig_addr = get_address(); // search through all valid SMBus addresses for (uint8_t i = BATT_SMBUS_ADDR_MIN; i <= BATT_SMBUS_ADDR_MAX; i++) { @@ -296,6 +297,9 @@ BATT_SMBUS::search() usleep(1); } + // restore original i2c address + set_address(orig_addr); + // display completion message if (found_slave) { warnx("Done."); @@ -367,7 +371,6 @@ BATT_SMBUS::cycle() new_report.voltage_v = ((float)tmp) / 1000.0f; // read current - usleep(1); uint8_t buff[4]; if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { @@ -376,7 +379,6 @@ BATT_SMBUS::cycle() // read battery design capacity if (_batt_design_capacity == 0) { - usleep(1); if (read_reg(BATT_SMBUS_DESIGN_CAPACITY, tmp) == OK) { _batt_design_capacity = tmp; } @@ -384,7 +386,6 @@ BATT_SMBUS::cycle() // read remaining capacity if (_batt_design_capacity > 0) { - usleep(1); if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { if (tmp < _batt_design_capacity) { new_report.discharged_mah = _batt_design_capacity - tmp; @@ -451,8 +452,6 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_ { uint8_t buff[max_len + 2]; // buffer to hold results - usleep(1); - // read bytes including PEC int ret = transfer(®, 1, buff, max_len + 2); From 969360c794e5d17cbf34539ee53329de94a63668 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 17:25:19 +0100 Subject: [PATCH 098/293] HMC5883 driver: Rotate before applying offsets. --- src/drivers/hmc5883/hmc5883.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 63b35fb19c61..90237b5ce168 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -848,6 +848,10 @@ HMC5883::collect() struct mag_report new_report; bool sensor_is_onboard = false; + float xraw_f; + float yraw_f; + float zraw_f; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); new_report.error_count = perf_event_count(_comms_errors); @@ -907,17 +911,21 @@ HMC5883::collect() report.x = -report.x; } - /* the standard external mag by 3DR has x pointing to the + /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x * and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + xraw_f = -report.y; + yraw_f = report.x; + zraw_f = report.z; // apply user specified rotation - rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; if (!(_pub_blocked)) { From 08c1d0c42d6b1647bbe122750af847b7632696f1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 2 Mar 2015 14:36:04 -0500 Subject: [PATCH 099/293] fix code style if trivial one line difference --- src/drivers/boards/aerocore/board_config.h | 2 +- src/drivers/boards/px4fmu-v1/board_config.h | 2 +- src/drivers/boards/px4fmu-v2/board_config.h | 2 +- src/drivers/boards/px4io-v1/px4io_init.c | 2 +- src/drivers/boards/px4io-v2/px4iov2_init.c | 2 +- src/drivers/drv_airspeed.h | 2 +- src/drivers/drv_blinkm.h | 2 +- src/drivers/drv_orb_dev.h | 2 +- src/drivers/drv_oreoled.h | 2 +- src/drivers/drv_pwm_input.h | 2 +- src/drivers/hmc5883/hmc5883.h | 2 +- src/drivers/ms5611/ms5611.h | 2 +- src/examples/fixedwing_control/params.c | 2 +- src/examples/fixedwing_control/params.h | 2 +- .../flow_position_estimator_params.c | 2 +- .../flow_position_estimator_params.h | 2 +- src/lib/conversion/rotation.h | 2 +- src/lib/launchdetection/LaunchDetector.h | 2 +- src/lib/rc/st24.h | 2 +- .../attitude_estimator_ekf_params.c | 1 + src/modules/commander/calibration_routines.h | 3 +- src/modules/fixedwing_backside/params.c | 2 +- src/modules/mavlink/mavlink_stream.cpp | 1 + src/modules/navigator/geofence.h | 2 +- .../position_estimator_inav/inertial_filter.c | 1 + src/modules/px4iofirmware/sbus.c | 2 +- src/modules/sensors/sensor_params.c | 2 +- src/modules/systemlib/circuit_breaker.cpp | 2 +- src/modules/systemlib/circuit_breaker.h | 2 +- src/modules/systemlib/rc_check.h | 2 +- src/modules/uORB/topics/esc_status.h | 2 +- src/modules/uORB/topics/fence.h | 2 +- .../demo_offboard_position_setpoints.cpp | 78 +++++++++++++++++++ src/systemcmds/tests/test_jig_voltages.c | 2 +- src/systemcmds/tests/test_rc.c | 2 +- src/systemcmds/tests/test_uart_send.c | 2 +- src/systemcmds/usb_connected/usb_connected.c | 2 +- 37 files changed, 115 insertions(+), 33 deletions(-) create mode 100644 src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 776a2071edfe..19518d73d9bf 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 882ec6aa2de0..17fa9c542ed7 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -52,7 +52,7 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include #include - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 273af1023a6e..6188e29ae19f 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4io-v1/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c index 8292da9e1c42..5a02a9716406 100644 --- a/src/drivers/boards/px4io-v1/px4io_init.c +++ b/src/drivers/boards/px4io-v1/px4io_init.c @@ -87,7 +87,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_RELAY1_EN); stm32_configgpio(GPIO_RELAY2_EN); - /* turn off - all leds are active low */ + /* turn off - all leds are active low */ stm32_gpiowrite(GPIO_LED1, true); stm32_gpiowrite(GPIO_LED2, true); stm32_gpiowrite(GPIO_LED3, true); diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 5c3343ccca0a..fd7eb33c3e93 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -126,7 +126,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ stm32_configgpio(GPIO_SBUS_OUTPUT); - + /* sbus output enable is active low - disable it by default */ stm32_gpiowrite(GPIO_SBUS_OENABLE, true); stm32_configgpio(GPIO_SBUS_OENABLE); diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index 2ff91d5d0084..fddef3626391 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -35,7 +35,7 @@ * @file drv_airspeed.h * * Airspeed driver interface. - * + * * @author Simon Wilks */ diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h index 7258c9e8411a..8568436d3596 100644 --- a/src/drivers/drv_blinkm.h +++ b/src/drivers/drv_blinkm.h @@ -60,7 +60,7 @@ /** play the numbered script in (arg), repeating forever */ #define BLINKM_PLAY_SCRIPT _BLINKMIOC(2) -/** +/** * Set the user script; (arg) is a pointer to an array of script lines, * where each line is an array of four bytes giving , , arg[0-2] * diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index e0b16fa5cd34..c1db6b534bd4 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -36,7 +36,7 @@ /** * @file drv_orb_dev.h - * + * * uORB published object driver. */ diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 0dcb10a7b1ef..00e36831841d 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -117,7 +117,7 @@ enum oreoled_macro { OREOLED_PARAM_MACRO_ENUM_COUNT }; -/* +/* structure passed to OREOLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless of the hardware scaling diff --git a/src/drivers/drv_pwm_input.h b/src/drivers/drv_pwm_input.h index 778d9fcf5b47..47b84e6e1e46 100644 --- a/src/drivers/drv_pwm_input.h +++ b/src/drivers/drv_pwm_input.h @@ -46,6 +46,6 @@ __BEGIN_DECLS /* * Initialise the timer */ -__EXPORT extern int pwm_input_main(int argc, char * argv[]); +__EXPORT extern int pwm_input_main(int argc, char *argv[]); __END_DECLS diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h index e91e91fc0996..9607fe61493f 100644 --- a/src/drivers/hmc5883/hmc5883.h +++ b/src/drivers/hmc5883/hmc5883.h @@ -50,4 +50,4 @@ /* interface factories */ extern device::Device *HMC5883_SPI_interface(int bus) weak_function; extern device::Device *HMC5883_I2C_interface(int bus) weak_function; -typedef device::Device* (*HMC5883_constructor)(int); +typedef device::Device *(*HMC5883_constructor)(int); diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index c8211b8dd1de..c946e38cb88c 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -82,4 +82,4 @@ extern bool crc4(uint16_t *n_prom); /* interface factories */ extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; -typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); +typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c index c2e94ff3d3d5..fbbd30201ecb 100644 --- a/src/examples/fixedwing_control/params.c +++ b/src/examples/fixedwing_control/params.c @@ -34,7 +34,7 @@ /* * @file params.c - * + * * Parameters for fixedwing demo */ diff --git a/src/examples/fixedwing_control/params.h b/src/examples/fixedwing_control/params.h index 4ae8e90ec461..49ffff44678f 100644 --- a/src/examples/fixedwing_control/params.h +++ b/src/examples/fixedwing_control/params.h @@ -34,7 +34,7 @@ /* * @file params.h - * + * * Definition of parameters for fixedwing example */ diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.c b/src/examples/flow_position_estimator/flow_position_estimator_params.c index ec3c3352d057..b56579787473 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.c @@ -35,7 +35,7 @@ /* * @file flow_position_estimator_params.c - * + * * Parameters for position estimator */ diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.h b/src/examples/flow_position_estimator/flow_position_estimator_params.h index f9a9bb303ba1..3ab4e560fc97 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.h +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.h @@ -35,7 +35,7 @@ /* * @file flow_position_estimator_params.h - * + * * Parameters for position estimator */ diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 917c7f830e43..13fe701dff00 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -118,7 +118,7 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); /** diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 4215b49d28f4..3b276c55630b 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -76,7 +76,7 @@ class __EXPORT LaunchDetector : public control::SuperBlock method is checked for further adavancing in the state machine (e.g. when to power up the motors) */ - LaunchMethod* launchMethods[1]; + LaunchMethod *launchMethods[1]; control::BlockParamInt launchdetection_on; control::BlockParamFloat throttlePreTakeoff; diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 454234601d0f..0cf732824bed 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -158,6 +158,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error */ __EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, - uint16_t *channels, uint16_t max_chan_count); + uint16_t *channels, uint16_t max_chan_count); __END_DECLS 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 5637ec102fea..e143f37b9b54 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -177,6 +177,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/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 3c8e49ee9ee6..ba4ca07ccbce 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -57,4 +57,5 @@ * @return 0 on success, 1 on failure */ int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius); \ No newline at end of file + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, + float *sphere_radius); \ No newline at end of file diff --git a/src/modules/fixedwing_backside/params.c b/src/modules/fixedwing_backside/params.c index db30af41603d..aa82a4f596d6 100644 --- a/src/modules/fixedwing_backside/params.c +++ b/src/modules/fixedwing_backside/params.c @@ -57,7 +57,7 @@ PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity // rate of climb -// this is what rate of climb is commanded (in m/s) +// this is what rate of climb is commanded (in m/s) // when the pitch stick is fully defelcted in simple mode PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 5b9e45d3ec70..57a92c8b58e5 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -80,6 +80,7 @@ MavlinkStream::update(const hrt_abstime t) if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); + if (const_rate()) { _last_sent = (t / _interval) * _interval; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 9d647cb68a9a..37a41b68a312 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -76,7 +76,7 @@ class Geofence : public control::SuperBlock * @return true: system is inside fence, false: system is outside fence */ bool 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); bool inside_polygon(double lat, double lon, float altitude); int clearDm(); diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index a36a4688d0ad..c70cbeb0ea79 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -15,6 +15,7 @@ void inertial_filter_predict(float dt, float x[2], float acc) if (!isfinite(acc)) { acc = 0.0f; } + x[0] += x[1] * dt + acc * dt * dt / 2.0f; x[1] += acc * dt; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index d76ec55f052f..9d2849090793 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -142,7 +142,7 @@ sbus1_output(uint16_t *values, uint16_t num_values) value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f); /*protect from out of bounds values and limit to 11 bits*/ - if (value > 0x07ff ) { + if (value > 0x07ff) { value = 0x07ff; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 5e04241fe3eb..d94bc9cffc87 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -643,7 +643,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); * * @group Sensor Calibration */ - PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); /** * Board rotation X (Roll) offset diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index ea478a60fa5c..f5ff0afd4bd1 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -45,7 +45,7 @@ #include #include -bool circuit_breaker_enabled(const char* breaker, int32_t magic) +bool circuit_breaker_enabled(const char *breaker, int32_t magic) { int32_t val; (void)PX4_PARAM_GET_BYNAME(breaker, &val); diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index c97dbc26ff69..c76e6c37f383 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -61,7 +61,7 @@ __BEGIN_DECLS -extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); +extern "C" __EXPORT bool circuit_breaker_enabled(const char *breaker, int32_t magic); __END_DECLS diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index e70b83cceedd..035f63bef4a4 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -39,7 +39,7 @@ #pragma once - __BEGIN_DECLS +__BEGIN_DECLS /** * Check the RC calibration diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index b45c49788d52..73fe0f9361f1 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -92,7 +92,7 @@ struct esc_status_s { struct { enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ - int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ + int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ float esc_current; /**< Current measured from current ESC [A] - if supported */ float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h index a61f078ba18b..43cee89fe81a 100644 --- a/src/modules/uORB/topics/fence.h +++ b/src/modules/uORB/topics/fence.h @@ -65,7 +65,7 @@ struct fence_vertex_s { * This is the position of a geofence * */ -struct fence_s { +struct fence_s { unsigned count; /**< number of actual vertices */ struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES]; }; diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp new file mode 100644 index 000000000000..125ceaddc75a --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file demo_offboard_position_Setpoints.cpp + * + * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL + * + * @author Thomas Gubler +*/ + +#include "demo_offboard_position_setpoints.h" + +#include +#include + +DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() : + _n(), + _local_position_sp_pub(_n.advertise("mavros/setpoint/local_position", 1)) +{ +} + + +int DemoOffboardPositionSetpoints::main() +{ + px4::Rate loop_rate(10); + + while (ros::ok()) { + loop_rate.sleep(); + ros::spinOnce(); + + /* Publish example offboard position setpoint */ + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.position.z = 1; + _local_position_sp_pub.publish(pose); + } + + return 0; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "demo_offboard_position_setpoints"); + DemoOffboardPositionSetpoints d; + return d.main(); +} diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c index 98a105cb3e00..554125302a96 100644 --- a/src/systemcmds/tests/test_jig_voltages.c +++ b/src/systemcmds/tests/test_jig_voltages.c @@ -162,7 +162,7 @@ int test_jig_voltages(int argc, char *argv[]) errout_with_dev: - if (fd != 0) close(fd); + if (fd != 0) { close(fd); } return ret; } diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index c9f9ef4398c2..3a8144077ccb 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -126,7 +126,7 @@ int test_rc(int argc, char *argv[]) warnx("TIMEOUT, less than 10 Hz updates"); (void)close(_rc_sub); return ERROR; - } + } } else { /* key pressed, bye bye */ diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c index 7e1e8d307d69..70a9c719e1c3 100644 --- a/src/systemcmds/tests/test_uart_send.c +++ b/src/systemcmds/tests/test_uart_send.c @@ -95,7 +95,7 @@ int test_uart_send(int argc, char *argv[]) /* input handling */ char *uart_name = "/dev/ttyS3"; - if (argc > 1) uart_name = argv[1]; + if (argc > 1) { uart_name = argv[1]; } /* assuming NuttShell is on UART1 (/dev/ttyS0) */ int test_uart = open(uart_name, O_RDWR | O_NONBLOCK | O_NOCTTY); // diff --git a/src/systemcmds/usb_connected/usb_connected.c b/src/systemcmds/usb_connected/usb_connected.c index 98bbbc4be958..530fee340552 100644 --- a/src/systemcmds/usb_connected/usb_connected.c +++ b/src/systemcmds/usb_connected/usb_connected.c @@ -52,5 +52,5 @@ __EXPORT int usb_connected_main(int argc, char *argv[]); int usb_connected_main(int argc, char *argv[]) { - return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0:1; + return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1; } From 4f3afaafa0e149d8a46152401afbdd87d28565bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Mar 2015 22:58:00 -0700 Subject: [PATCH 100/293] Mag drivers: Temp support --- src/drivers/drv_mag.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 1d2a5df701ee..c010a26fa975 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -63,6 +63,7 @@ struct mag_report { float z; float range_ga; float scaling; + float temperature; int16_t x_raw; int16_t y_raw; From b34a394fbdf7cccce9f8a33ee948dc7b9a822aaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Mar 2015 22:57:47 -0700 Subject: [PATCH 101/293] HMC5883: Temp dummy value --- src/drivers/hmc5883/hmc5883.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 90237b5ce168..1bd70f3c0585 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -927,6 +927,9 @@ HMC5883::collect() /* z remains z */ new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; + /* this sensor does not measure temperature, output a constant value */ + new_report.temperature = 0.0f; + if (!(_pub_blocked)) { if (_mag_topic != -1) { From 27253468896845da36ed9505b0f7feee6a7fcb44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Feb 2015 12:22:45 +0100 Subject: [PATCH 102/293] LSM303D: Return argument in right format --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 1e76599b712d..c95df330b958 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -924,7 +924,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) } case ACCELIOCGLOWPASS: - return _accel_filter_x.get_cutoff_freq(); + return static_cast(_accel_filter_x.get_cutoff_freq()); case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ From 6c6bf514b67c39a5f566ea2d57a5e6c1b7047d13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Feb 2015 16:18:21 +0100 Subject: [PATCH 103/293] LSM303D: Update comment why we report as internal always --- src/drivers/lsm303d/lsm303d.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index c95df330b958..b2d345371528 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1078,7 +1078,10 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return mag_self_test(); case MAGIOCGEXTERNAL: - /* no external mag board yet */ + /* Even if this sensor is on the "external" SPI bus + * it is still fixed to the autopilot assembly, + * so always return 0. + */ return 0; default: From d12440efbd511c5537b64c3c3fe81bb82f360edf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Mar 2015 17:25:55 +0100 Subject: [PATCH 104/293] LSM303D driver: Rotate before applying offsets. --- src/drivers/lsm303d/lsm303d.cpp | 33 +++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index b2d345371528..f174cede098d 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1531,9 +1531,16 @@ LSM303D::measure() accel_report.y_raw = raw_accel_report.y; accel_report.z_raw = raw_accel_report.z; - float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = raw_accel_report.x; + float yraw_f = raw_accel_report.y; + float zraw_f = raw_accel_report.z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; /* we have logs where the accelerometers get stuck at a fixed @@ -1569,9 +1576,6 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); - // apply user specified rotation - rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z); - accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1638,9 +1642,17 @@ LSM303D::mag_measure() mag_report.x_raw = raw_mag_report.x; mag_report.y_raw = raw_mag_report.y; mag_report.z_raw = raw_mag_report.z; - mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + + float xraw_f = mag_report.x_raw; + float yraw_f = mag_report.y_raw; + float zraw_f = mag_report.z_raw; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; mag_report.scaling = _mag_range_scale; mag_report.range_ga = (float)_mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); @@ -1649,9 +1661,6 @@ LSM303D::mag_measure() // seems to be a signed offset from 25 degrees C in units of 0.125C _last_temperature = 25 + (raw_mag_report.temperature*0.125f); - // apply user specified rotation - rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z); - _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ From 03f7268055ddc5e9e56539d298268118c9ea0b1e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Mar 2015 22:57:32 -0700 Subject: [PATCH 105/293] LSM303D: Temp support --- src/drivers/lsm303d/lsm303d.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index f174cede098d..2a49b439b48f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1647,7 +1647,7 @@ LSM303D::mag_measure() float yraw_f = mag_report.y_raw; float zraw_f = mag_report.z_raw; - // apply user specified rotation + /* apply user specified rotation */ rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; @@ -1657,13 +1657,14 @@ LSM303D::mag_measure() mag_report.range_ga = (float)_mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - // remember the temperature. The datasheet isn't clear, but it - // seems to be a signed offset from 25 degrees C in units of 0.125C - _last_temperature = 25 + (raw_mag_report.temperature*0.125f); + /* remember the temperature. The datasheet isn't clear, but it + * seems to be a signed offset from 25 degrees C in units of 0.125C + */ + _last_temperature = 25 + (raw_mag_report.temperature * 0.125f); + mag_report.temperature = _last_temperature; _mag_reports->force(&mag_report); - /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -1700,7 +1701,7 @@ LSM303D::print_info() (unsigned)_checked_values[i]); } } - ::printf("temperature: %.2f\n", _last_temperature); + ::printf("temperature: %.2f\n", (double)_last_temperature); } void From da8b3082f2985109f187ac56a22b37052259bb42 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Mar 2015 12:18:24 -0700 Subject: [PATCH 106/293] drv_mag: added ioctl to control temperature compensation --- src/drivers/drv_mag.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index c010a26fa975..a413fb217a2d 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -129,4 +129,7 @@ ORB_DECLARE(sensor_mag); /** determine if mag is external or onboard */ #define MAGIOCGEXTERNAL _MAGIOC(11) +/** enable/disable temperature compensation */ +#define MAGIOCSTEMPCOMP _MAGIOC(12) + #endif /* _DRV_MAG_H */ From 0b1fa0e528d0add85a0e8f1241d96fde0206afd2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Mar 2015 16:37:06 -0700 Subject: [PATCH 107/293] hmc5883: added support for temperature compensation added "hmc5883 tempon" and "hmc5883 tempoff" to enable/disable --- src/drivers/hmc5883/hmc5883.cpp | 109 ++++++++++++++++++++++++++++++-- 1 file changed, 105 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 1bd70f3c0585..0f54cb6d4492 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -94,6 +94,10 @@ #define ADDR_DATA_OUT_Y_LSB 0x08 #define ADDR_STATUS 0x09 +/* temperature on hmc5983 only */ +#define ADDR_TEMP_OUT_MSB 0x31 +#define ADDR_TEMP_OUT_LSB 0x32 + /* modes not changeable outside of driver */ #define HMC5883L_MODE_NORMAL (0 << 0) /* default */ #define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */ @@ -110,6 +114,8 @@ #define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */ #define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ +#define HMC5983_TEMP_SENSOR_ENABLE (1 << 7) + enum HMC5883_BUS { HMC5883_BUS_ALL = 0, HMC5883_BUS_I2C_INTERNAL, @@ -218,6 +224,11 @@ class HMC5883 : public device::CDev */ int set_excitement(unsigned enable); + /** + * enable hmc5983 temperature compensation + */ + int set_temperature_compensation(unsigned enable); + /** * Set the sensor range. * @@ -722,6 +733,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) debug("MAGIOCGEXTERNAL in main driver"); return _interface->ioctl(cmd, dummy); + case MAGIOCSTEMPCOMP: + return set_temperature_compensation(arg); + case DEVIOCGDEVICEID: return _interface->ioctl(cmd, dummy); @@ -847,6 +861,7 @@ HMC5883::collect() perf_begin(_sample_perf); struct mag_report new_report; bool sensor_is_onboard = false; + uint8_t raw_temperature[2]; float xraw_f; float yraw_f; @@ -888,6 +903,20 @@ HMC5883::collect() goto out; } + /* get measurements from the device */ + new_report.temperature = 0; + if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) { + /* if temperature compensation is enabled read the + * temperature too */ + ret = _interface->read(ADDR_TEMP_OUT_MSB, + raw_temperature, sizeof(raw_temperature)); + if (ret == OK) { + int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + + raw_temperature[1]; + new_report.temperature = 25 + (temp16 / (16*8.0f)); + } + } + /* * RAW outputs * @@ -927,9 +956,6 @@ HMC5883::collect() /* z remains z */ new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; - /* this sensor does not measure temperature, output a constant value */ - new_report.temperature = 0.0f; - if (!(_pub_blocked)) { if (_mag_topic != -1) { @@ -1234,6 +1260,50 @@ int HMC5883::set_excitement(unsigned enable) return !(_conf_reg == conf_reg_ret); } + +/* + enable/disable temperature compensation on the HMC5983 + + Unfortunately we don't yet know of a way to auto-detect the + difference between the HMC5883 and HMC5983. Both of them do + temperature sensing, but only the 5983 does temperature + compensation. We have noy yet found a behaviour that can be reliably + distinguished by reading registers to know which type a particular + sensor is + */ +int HMC5883::set_temperature_compensation(unsigned enable) +{ + int ret; + /* get current config */ + ret = read_reg(ADDR_CONF_A, _conf_reg); + + if (OK != ret) { + perf_count(_comms_errors); + return -EIO; + } + + if (enable != 0) { + _conf_reg |= HMC5983_TEMP_SENSOR_ENABLE; + } else { + _conf_reg &= ~HMC5983_TEMP_SENSOR_ENABLE; + } + + ret = write_reg(ADDR_CONF_A, _conf_reg); + + if (OK != ret) { + perf_count(_comms_errors); + return -EIO; + } + + uint8_t conf_reg_ret = 0; + if (read_reg(ADDR_CONF_A, conf_reg_ret) != OK) { + perf_count(_comms_errors); + return -EIO; + } + + return conf_reg_ret == _conf_reg; +} + int HMC5883::write_reg(uint8_t reg, uint8_t val) { @@ -1276,6 +1346,7 @@ HMC5883::print_info() printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, (double)(1.0f/_range_scale), (double)_range_ga); + printf("temperature %.2f\n", (double)_last_report.temperature); _reports->print_info("report queue"); } @@ -1318,6 +1389,7 @@ void test(enum HMC5883_BUS busid); void reset(enum HMC5883_BUS busid); int info(enum HMC5883_BUS busid); int calibrate(enum HMC5883_BUS busid); +void temp_enable(HMC5883_BUS busid, bool enable); void usage(); /** @@ -1560,6 +1632,27 @@ reset(enum HMC5883_BUS busid) exit(0); } + +/** + * enable/disable temperature compensation + */ +void +temp_enable(enum HMC5883_BUS busid, bool enable) +{ + struct hmc5883_bus_option &bus = find_bus(busid); + const char *path = bus.devpath; + + int fd = open(path, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) + err(1, "set temperature compensation failed"); + + exit(0); +} + /** * Print a little info about the driver. */ @@ -1652,6 +1745,14 @@ hmc5883_main(int argc, char *argv[]) if (!strcmp(verb, "reset")) hmc5883::reset(busid); + /* + * enable/disable temperature compensation + */ + if (!strcmp(verb, "tempoff")) + hmc5883::temp_enable(busid, false); + if (!strcmp(verb, "tempon")) + hmc5883::temp_enable(busid, true); + /* * Print driver information. */ @@ -1670,5 +1771,5 @@ hmc5883_main(int argc, char *argv[]) } } - errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'"); + errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate', 'tempoff', 'tempon' or 'info'"); } From 3d6fd6baf5a90010aed084aa7d3c13d1b7fa059e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 Apr 2015 11:09:15 -0700 Subject: [PATCH 108/293] hmc5883: try to cope with genuine 5883 parts if we can't read the temperature registers 10 times then disable the feature. --- src/drivers/hmc5883/hmc5883.cpp | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0f54cb6d4492..08be5245c282 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -182,6 +182,7 @@ class HMC5883 : public device::CDev uint8_t _range_bits; uint8_t _conf_reg; + uint8_t _temperature_error_count; /** * Initialise the automatic measurement state machine and start it. @@ -369,7 +370,8 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota _rotation(rotation), _last_report{0}, _range_bits(0), - _conf_reg(0) + _conf_reg(0), + _temperature_error_count(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; @@ -914,6 +916,18 @@ HMC5883::collect() int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + raw_temperature[1]; new_report.temperature = 25 + (temp16 / (16*8.0f)); + _temperature_error_count = 0; + } else { + _temperature_error_count++; + if (_temperature_error_count == 10) { + /* + it probably really is a old HMC5883, + and can't do temperature. Disable it + */ + _temperature_error_count = 0; + debug("disabling temperature compensation"); + set_temperature_compensation(0); + } } } @@ -1270,6 +1284,16 @@ int HMC5883::set_excitement(unsigned enable) compensation. We have noy yet found a behaviour that can be reliably distinguished by reading registers to know which type a particular sensor is + + update: Current best guess is that many sensors marked HMC5883L on + the package are actually 5983 but without temperature compensation + tables. Reading the temperature works, but the mag field is not + automatically adjusted for temperature. We suspect that there may be + some early 5883L parts that don't have the temperature sensor at + all, although we haven't found one yet. The code that reads the + temperature looks for 10 failed transfers in a row and disables the + temperature sensor if that happens. It is hoped that this copes with + the genuine 5883L parts. */ int HMC5883::set_temperature_compensation(unsigned enable) { From fff0935d90c71e7db694eb88b0b87e912a78806e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 Apr 2015 11:51:20 -0700 Subject: [PATCH 109/293] hmc5883: added -T option to enable temperature compensation this also fixes the behaviour of the -C option --- src/drivers/hmc5883/hmc5883.cpp | 40 +++++++++++++++------------------ 1 file changed, 18 insertions(+), 22 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 08be5245c282..ab70bf5b0674 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1413,7 +1413,7 @@ void test(enum HMC5883_BUS busid); void reset(enum HMC5883_BUS busid); int info(enum HMC5883_BUS busid); int calibrate(enum HMC5883_BUS busid); -void temp_enable(HMC5883_BUS busid, bool enable); +int temp_enable(HMC5883_BUS busid, bool enable); void usage(); /** @@ -1478,9 +1478,6 @@ start(enum HMC5883_BUS busid, enum Rotation rotation) if (!started) errx(1, "driver start failed"); - - // one or more drivers started OK - exit(0); } /** @@ -1625,12 +1622,7 @@ int calibrate(enum HMC5883_BUS busid) close(fd); - if (ret == OK) { - errx(0, "PASS"); - - } else { - errx(1, "FAIL"); - } + return ret; } /** @@ -1660,7 +1652,7 @@ reset(enum HMC5883_BUS busid) /** * enable/disable temperature compensation */ -void +int temp_enable(enum HMC5883_BUS busid, bool enable) { struct hmc5883_bus_option &bus = find_bus(busid); @@ -1674,7 +1666,8 @@ temp_enable(enum HMC5883_BUS busid, bool enable) if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) err(1, "set temperature compensation failed"); - exit(0); + close(fd); + return 0; } /** @@ -1712,8 +1705,9 @@ hmc5883_main(int argc, char *argv[]) enum HMC5883_BUS busid = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; + bool temp_compensation = false; - while ((ch = getopt(argc, argv, "XISR:C")) != EOF) { + while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(optarg); @@ -1732,6 +1726,9 @@ hmc5883_main(int argc, char *argv[]) case 'C': calibrate = true; break; + case 'T': + temp_compensation = true; + break; default: hmc5883::usage(); exit(0); @@ -1745,16 +1742,15 @@ hmc5883_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { hmc5883::start(busid, rotation); - if (calibrate) { - if (hmc5883::calibrate(busid) == 0) { - errx(0, "calibration successful"); - - } else { - errx(1, "calibration failed"); - } - } else { - exit(0); + if (calibrate && hmc5883::calibrate(busid) != 0) { + errx(1, "calibration failed"); + } + if (temp_compensation) { + // we consider failing to setup temperature + // compensation as non-fatal + hmc5883::temp_enable(busid, true); } + exit(0); } /* From 27bf1102e1486223cff3c1ac462a26c75b1ef519 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Feb 2015 16:17:46 +0100 Subject: [PATCH 110/293] HMC5883: Always report as internal sensor in SPI mode, since the sensor is fixed to the autopilot assembly. --- src/drivers/hmc5883/hmc5883_spi.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp index aec990ca884b..b88310539a8b 100644 --- a/src/drivers/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -138,15 +138,12 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) switch (operation) { case MAGIOCGEXTERNAL: - -#ifdef PX4_SPI_BUS_EXT - if (_bus == PX4_SPI_BUS_EXT) { - return 1; - } else -#endif - { - return 0; - } + /* + * Even if this sensor is on the external SPI + * bus it is still internal to the autopilot + * assembly, so always return 0 for internal. + */ + return 0; case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); From 54950efc0a7e7d5fa4e98f8bb2da9ea21d1dcd97 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 Apr 2015 14:27:47 -0700 Subject: [PATCH 111/293] hmc5883: read the temperature every 10 samples when enabled --- src/drivers/hmc5883/hmc5883.cpp | 52 +++++++++++++++++++++------------ 1 file changed, 33 insertions(+), 19 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index ab70bf5b0674..3a3848446d41 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -182,6 +182,7 @@ class HMC5883 : public device::CDev uint8_t _range_bits; uint8_t _conf_reg; + uint8_t _temperature_counter; uint8_t _temperature_error_count; /** @@ -371,6 +372,7 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota _last_report{0}, _range_bits(0), _conf_reg(0), + _temperature_counter(0), _temperature_error_count(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; @@ -863,7 +865,6 @@ HMC5883::collect() perf_begin(_sample_perf); struct mag_report new_report; bool sensor_is_onboard = false; - uint8_t raw_temperature[2]; float xraw_f; float yraw_f; @@ -908,26 +909,39 @@ HMC5883::collect() /* get measurements from the device */ new_report.temperature = 0; if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) { - /* if temperature compensation is enabled read the - * temperature too */ - ret = _interface->read(ADDR_TEMP_OUT_MSB, - raw_temperature, sizeof(raw_temperature)); - if (ret == OK) { - int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + - raw_temperature[1]; - new_report.temperature = 25 + (temp16 / (16*8.0f)); - _temperature_error_count = 0; - } else { - _temperature_error_count++; - if (_temperature_error_count == 10) { - /* - it probably really is a old HMC5883, - and can't do temperature. Disable it - */ + /* + if temperature compensation is enabled read the + temperature too. + + We read the temperature every 10 samples to avoid + excessive I2C traffic + */ + if (_temperature_counter++ == 10) { + uint8_t raw_temperature[2]; + + _temperature_counter = 0; + + ret = _interface->read(ADDR_TEMP_OUT_MSB, + raw_temperature, sizeof(raw_temperature)); + if (ret == OK) { + int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + + raw_temperature[1]; + new_report.temperature = 25 + (temp16 / (16*8.0f)); _temperature_error_count = 0; - debug("disabling temperature compensation"); - set_temperature_compensation(0); + } else { + _temperature_error_count++; + if (_temperature_error_count == 10) { + /* + it probably really is an old HMC5883, + and can't do temperature. Disable it + */ + _temperature_error_count = 0; + debug("disabling temperature compensation"); + set_temperature_compensation(0); + } } + } else { + new_report.temperature = _last_report.temperature; } } From e0f266b6814687efe9bb5e1dc18ea7e67b38de9f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 8 Apr 2015 22:33:07 -0700 Subject: [PATCH 112/293] batt_smbus: add ioctl to return batt capacity Also use full charge capacity instead of design capacity so that an old battery's capacity will appear as lower than its original capacity but it will still report 100% charged after charging --- src/drivers/batt_smbus/batt_smbus.cpp | 53 ++++++++++++++++++++++----- src/drivers/drv_batt_smbus.h | 10 +++++ 2 files changed, 53 insertions(+), 10 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 2008e0c7ce12..d7b128980a6f 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -85,6 +85,7 @@ #define BATT_SMBUS_TEMP 0x08 ///< temperature register #define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register #define BATT_SMBUS_REMAINING_CAPACITY 0x0f ///< predicted remaining battery capacity as a percentage +#define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged #define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register #define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register @@ -115,6 +116,11 @@ class BATT_SMBUS : public device::I2C */ virtual int init(); + /** + * ioctl for retrieving battery capacity and time to empty + */ + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + /** * Test device * @@ -180,7 +186,7 @@ class BATT_SMBUS : public device::I2C orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID uint64_t _start_time; ///< system time we first attempt to communicate with battery - uint16_t _batt_design_capacity; ///< battery's design capacity in mAh (0 means unknown) + uint16_t _batt_capacity; ///< battery's design capacity in mAh (0 means unknown) }; namespace @@ -200,7 +206,7 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : _batt_topic(-1), _batt_orb_id(nullptr), _start_time(0), - _batt_design_capacity(0) + _batt_capacity(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -250,6 +256,31 @@ BATT_SMBUS::init() return ret; } +int +BATT_SMBUS::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -ENODEV; + + switch (cmd) { + case BATT_SMBUS_GET_CAPACITY: + + /* return battery capacity as uint16 */ + if (_enabled) { + *((uint16_t *)arg) = _batt_capacity; + ret = OK; + } + + break; + + default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + int BATT_SMBUS::test() { @@ -266,7 +297,8 @@ BATT_SMBUS::test() if (updated) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { - warnx("V=%4.2f C=%4.2f DismAh=%4.2f", (float)status.voltage_v, (float)status.current_a, (float)status.discharged_mah); + warnx("V=%4.2f C=%4.2f DismAh=%4.2f Cap:%d", (float)status.voltage_v, (float)status.current_a, + (float)status.discharged_mah, (int)_batt_capacity); } } @@ -374,21 +406,22 @@ BATT_SMBUS::cycle() uint8_t buff[4]; if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { - new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | (uint32_t)buff[0])) / 1000.0f; + new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | + (uint32_t)buff[0])) / 1000.0f; } // read battery design capacity - if (_batt_design_capacity == 0) { - if (read_reg(BATT_SMBUS_DESIGN_CAPACITY, tmp) == OK) { - _batt_design_capacity = tmp; + if (_batt_capacity == 0) { + if (read_reg(BATT_SMBUS_FULL_CHARGE_CAPACITY, tmp) == OK) { + _batt_capacity = tmp; } } // read remaining capacity - if (_batt_design_capacity > 0) { + if (_batt_capacity > 0) { if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { - if (tmp < _batt_design_capacity) { - new_report.discharged_mah = _batt_design_capacity - tmp; + if (tmp < _batt_capacity) { + new_report.discharged_mah = _batt_capacity - tmp; } } } diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h index f12e2bfb3a7f..57af0a0b6895 100644 --- a/src/drivers/drv_batt_smbus.h +++ b/src/drivers/drv_batt_smbus.h @@ -45,3 +45,13 @@ /* device path */ #define BATT_SMBUS0_DEVICE_PATH "/dev/batt_smbus0" + +/* + * ioctl() definitions + */ + +#define _BATT_SMBUS_IOCBASE (0x2e00) +#define _BATT_SMBUS_IOC(_n) (_IOC(_BATT_SMBUS_IOCBASE, _n)) + +/** retrieve battery capacity */ +#define BATT_SMBUS_GET_CAPACITY _BATT_SMBUS_IOC(1) From ef546f6525bad6c1afe853ea46962a4b912b294e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 May 2015 10:45:29 +1000 Subject: [PATCH 113/293] lsm303d: run sampling 200usec faster to avoid aliasing this runs the sampling of the accelerometer 200usec faster than requested and then throw away duplicates using the accelerometer status register data ready bit. This avoids aliasing due to drift in the stm32 clock compared to the lsm303d clock --- src/drivers/lsm303d/lsm303d.cpp | 46 +++++++++++++++++++-------------- 1 file changed, 26 insertions(+), 20 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 2a49b439b48f..badaecf4d8af 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -196,6 +196,7 @@ static const int ERROR = -1; #define REG7_CONT_MODE_M ((0<<1) | (0<<0)) +#define REG_STATUS_A_NEW_ZYXADA 0x08 #define INT_CTRL_M 0x12 #define INT_SRC_M 0x13 @@ -217,6 +218,14 @@ static const int ERROR = -1; #define EXTERNAL_BUS 0 #endif +/* + we set the timer interrupt to run a bit faster than the desired + sample rate and then throw away duplicates using the data ready bit. + This time reduction is enough to cope with worst case timing jitter + due to other timers + */ +#define LSM303D_TIMER_REDUCTION 200 + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -289,9 +298,9 @@ class LSM303D : public device::SPI perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; - perf_counter_t _accel_reschedules; perf_counter_t _bad_registers; perf_counter_t _bad_values; + perf_counter_t _accel_duplicates; uint8_t _register_wait; @@ -561,9 +570,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), _bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")), _bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_values")), + _accel_duplicates(perf_alloc(PC_COUNT, "lsm303d_accel_duplicates")), _register_wait(0), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -622,7 +631,7 @@ LSM303D::~LSM303D() perf_free(_mag_sample_perf); perf_free(_bad_registers); perf_free(_bad_values); - perf_free(_accel_reschedules); + perf_free(_accel_duplicates); } int @@ -874,7 +883,9 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = ticks; + _call_accel_interval = ticks; + + _accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ if (want_start) @@ -1395,7 +1406,10 @@ LSM303D::start() _mag_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); + hrt_call_every(&_accel_call, + 1000, + _call_accel_interval - LSM303D_TIMER_REDUCTION, + (hrt_callout)&LSM303D::measure_trampoline, this); hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); } @@ -1473,20 +1487,6 @@ LSM303D::measure() check_registers(); - // if the accel doesn't have any data ready then re-schedule - // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value. - // Note that DRDY is not available when the lsm303d is - // connected on the external bus -#ifdef GPIO_EXTI_ACCEL_DRDY - if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { - perf_count(_accel_reschedules); - hrt_call_delay(&_accel_call, 100); - perf_end(_accel_sample_perf); - return; - } -#endif - if (_register_wait != 0) { // we are waiting for some good transfers before using // the sensor again. @@ -1500,6 +1500,12 @@ LSM303D::measure() raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + if (!(raw_accel_report.status & REG_STATUS_A_NEW_ZYXADA)) { + perf_end(_accel_sample_perf); + perf_count(_accel_duplicates); + return; + } + /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -1688,7 +1694,7 @@ LSM303D::print_info() perf_print_counter(_mag_sample_perf); perf_print_counter(_bad_registers); perf_print_counter(_bad_values); - perf_print_counter(_accel_reschedules); + perf_print_counter(_accel_duplicates); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); ::printf("checked_next: %u\n", _checked_next); From 70f4e529ed88472ccb0ab40de047b7bbb26a9476 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 May 2015 10:50:37 +1000 Subject: [PATCH 114/293] mpu6000: sample at 200usec faster rate to avoid aliasing this runs the mpu6000 200usec faster than requested then detects and disccards duplicates by comparing accel values. This avoids a nasty aliasing issue due to clock drift between the stm32 and mpu6000 --- src/drivers/mpu6000/mpu6000.cpp | 60 +++++++++++++++++++++++++++++---- 1 file changed, 54 insertions(+), 6 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 7bbd717bad1e..2181183b3b01 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -189,6 +189,14 @@ #define MPU6000_LOW_BUS_SPEED 1000*1000 #define MPU6000_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */ +/* + we set the timer interrupt to run a bit faster than the desired + sample rate and then throw away duplicates by comparing + accelerometer values. This time reduction is enough to cope with + worst case timing jitter due to other timers + */ +#define MPU6000_TIMER_REDUCTION 200 + class MPU6000_gyro; class MPU6000 : public device::SPI @@ -259,6 +267,7 @@ class MPU6000 : public device::SPI perf_counter_t _bad_registers; perf_counter_t _good_transfers; perf_counter_t _reset_retries; + perf_counter_t _duplicates; perf_counter_t _system_latency_perf; perf_counter_t _controller_latency_perf; @@ -289,6 +298,10 @@ class MPU6000 : public device::SPI // last temperature reading for print_info() float _last_temperature; + // keep last accel reading for duplicate detection + uint16_t _last_accel[3]; + bool _got_duplicate; + /** * Start automatic measurement. */ @@ -512,6 +525,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), + _duplicates(perf_alloc(PC_COUNT, "mpu6000_duplicates")), _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _register_wait(0), @@ -525,7 +539,9 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _rotation(rotation), _checked_next(0), _in_factory_test(false), - _last_temperature(0) + _last_temperature(0), + _last_accel{}, + _got_duplicate(false) { // disable debug() calls _debug_enabled = false; @@ -579,6 +595,8 @@ MPU6000::~MPU6000() perf_free(_bad_transfers); perf_free(_bad_registers); perf_free(_good_transfers); + perf_free(_reset_retries); + perf_free(_duplicates); } int @@ -1210,7 +1228,15 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; + _call_interval = ticks; + + /* + set call interval faster then the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu6000 clock + */ + _call.period = _call_interval - MPU6000_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ if (want_start) @@ -1501,7 +1527,10 @@ MPU6000::start() _gyro_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this); + hrt_call_every(&_call, + 1000, + _call_interval-MPU6000_TIMER_REDUCTION, + (hrt_callout)&MPU6000::measure_trampoline, this); } void @@ -1603,14 +1632,32 @@ MPU6000::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - check_registers(); - // sensor transfer at high clock speed set_frequency(MPU6000_HIGH_BUS_SPEED); if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; + check_registers(); + + /* + see if this is duplicate accelerometer data. Note that we + can't use the data ready interrupt status bit in the status + register as that also goes high on new gyro data, and when + we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being + sampled at 8kHz, so we would incorrectly think we have new + data when we are in fact getting duplicate accelerometer data. + */ + if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { + // it isn't new data - wait for next timer + perf_end(_sample_perf); + perf_count(_duplicates); + _got_duplicate = true; + return; + } + memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); + _got_duplicate = false; + /* * Convert from big to little endian */ @@ -1783,6 +1830,7 @@ MPU6000::print_info() perf_print_counter(_bad_registers); perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); + perf_print_counter(_duplicates); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); ::printf("checked_next: %u\n", _checked_next); @@ -1795,7 +1843,7 @@ MPU6000::print_info() (unsigned)_checked_values[i]); } } - ::printf("temperature: %.1f\n", _last_temperature); + ::printf("temperature: %.1f\n", (double)_last_temperature); } void From 548f37a758fa3933a5549f3bfbfb1437340e967c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 May 2015 13:26:08 +1000 Subject: [PATCH 115/293] l3gd20: follow same pattern as lsm303d for duplicate rejection --- src/drivers/l3gd20/l3gd20.cpp | 57 +++++++++++++++-------------------- 1 file changed, 24 insertions(+), 33 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index e577602a6d30..710451938637 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -189,6 +189,14 @@ static const int ERROR = -1; #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG #endif +/* + we set the timer interrupt to run a bit faster than the desired + sample rate and then throw away duplicates using the data ready bit. + This time reduction is enough to cope with worst case timing jitter + due to other timers + */ +#define L3GD20_TIMER_REDUCTION 200 + extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI @@ -237,9 +245,9 @@ class L3GD20 : public device::SPI unsigned _read; perf_counter_t _sample_perf; - perf_counter_t _reschedules; perf_counter_t _errors; perf_counter_t _bad_registers; + perf_counter_t _duplicates; uint8_t _register_wait; @@ -412,9 +420,9 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _orientation(SENSOR_BOARD_ROTATION_DEFAULT), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), - _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")), + _duplicates(perf_alloc(PC_COUNT, "l3gd20_duplicates")), _register_wait(0), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), @@ -451,9 +459,9 @@ L3GD20::~L3GD20() /* delete the perf counter */ perf_free(_sample_perf); - perf_free(_reschedules); perf_free(_errors); perf_free(_bad_registers); + perf_free(_duplicates); } int @@ -610,7 +618,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; + _call_interval = ticks; + + _call.period = _call_interval - L3GD20_TIMER_REDUCTION; /* adjust filters */ float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); @@ -871,7 +881,10 @@ L3GD20::start() _reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); + hrt_call_every(&_call, + 1000, + _call_interval - L3GD20_TIMER_REDUCTION, + (hrt_callout)&L3GD20::measure_trampoline, this); } void @@ -936,12 +949,6 @@ L3GD20::measure_trampoline(void *arg) dev->measure(); } -#ifdef GPIO_EXTI_GYRO_DRDY -# define L3GD20_USE_DRDY 1 -#else -# define L3GD20_USE_DRDY 0 -#endif - void L3GD20::check_registers(void) { @@ -991,33 +998,17 @@ L3GD20::measure() check_registers(); -#if L3GD20_USE_DRDY - // if the gyro doesn't have any data ready then re-schedule - // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value - if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { - perf_count(_reschedules); - hrt_call_delay(&_call, 100); - perf_end(_sample_perf); - return; - } -#endif - /* fetch data from the sensor */ memset(&raw_report, 0, sizeof(raw_report)); raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); -#if L3GD20_USE_DRDY - if (_bus == PX4_SPI_BUS_SENSORS && (raw_report.status & 0xF) != 0xF) { - /* - we waited for DRDY, but did not see DRDY on all axes - when we captured. That means a transfer error of some sort - */ - perf_count(_errors); - return; + if (!(raw_report.status & STATUS_ZYXDA)) { + perf_end(_sample_perf); + perf_count(_duplicates); + return; } -#endif + /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -1104,9 +1095,9 @@ L3GD20::print_info() { printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); - perf_print_counter(_reschedules); perf_print_counter(_errors); perf_print_counter(_bad_registers); + perf_print_counter(_duplicates); _reports->print_info("report queue"); ::printf("checked_next: %u\n", _checked_next); for (uint8_t i=0; i Date: Mon, 18 May 2015 20:51:14 +0900 Subject: [PATCH 116/293] l3gd20: faster gyro interrupts --- src/drivers/l3gd20/l3gd20.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 710451938637..36872ea2187a 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -195,7 +195,7 @@ static const int ERROR = -1; This time reduction is enough to cope with worst case timing jitter due to other timers */ -#define L3GD20_TIMER_REDUCTION 200 +#define L3GD20_TIMER_REDUCTION 600 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } From 595b205cafbb4480c2aafb9cf69d0da1e0772cd5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 31 May 2015 14:34:40 +1000 Subject: [PATCH 117/293] batt_smbus: fixed build for new ringbuffer class --- src/drivers/batt_smbus/batt_smbus.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index c9c4e826733d..fb3c524a8224 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -181,7 +181,7 @@ class BATT_SMBUS : public device::I2C // internal variables bool _enabled; ///< true if we have successfully connected to battery work_s _work; ///< work queue for scheduling reads - RingBuffer *_reports; ///< buffer of recorded voltages, currents + ringbuffer::RingBuffer *_reports; ///< buffer of recorded voltages, currents struct battery_status_s _last_report; ///< last published report, used for test() orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID @@ -239,7 +239,7 @@ BATT_SMBUS::init() } else { // allocate basic report buffers - _reports = new RingBuffer(2, sizeof(struct battery_status_s)); + _reports = new ringbuffer::RingBuffer(2, sizeof(struct battery_status_s)); if (_reports == nullptr) { ret = ENOTTY; From 345500c040ffe318ba2290b2da39c610000a6677 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 31 May 2015 14:35:36 +1000 Subject: [PATCH 118/293] uavcan: setup defaults for node ID and bitrate The ArduPilot build does not use the PX4 parameter system, so the parameter calls fail --- src/modules/uavcan/uavcan_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8104b4b28942..5b22d4b3c817 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -705,7 +705,7 @@ int uavcan_main(int argc, char *argv[]) } // Node ID - int32_t node_id = 0; + int32_t node_id = 1; (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id); if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { @@ -714,7 +714,7 @@ int uavcan_main(int argc, char *argv[]) } // CAN bitrate - int32_t bitrate = 0; + int32_t bitrate = 1000000; (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate); // Start From f9ad47cac73866e434efd2add75cd407c0acccd7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 31 May 2015 14:36:01 +1000 Subject: [PATCH 119/293] motor_test: nullptr is in C++, not C --- src/systemcmds/motor_test/motor_test.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 2108b2004eda..87ea1b13eabb 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -59,7 +59,7 @@ __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 = nullptr; +static orb_advert_t _test_motor_pub = NULL; void motor_test(unsigned channel, float value) { @@ -69,7 +69,7 @@ void motor_test(unsigned channel, float value) _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub != nullptr) { + if (_test_motor_pub != NULL) { /* publish test state */ orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); } else { From 13cf41e83fcdeac08ec6f54d87f239bb963f88b8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 31 May 2015 14:39:04 +1000 Subject: [PATCH 120/293] uavcan: use UAVCAN_DIR for module path This allows the use of a different uavcan directory, as ArduPilot does --- src/modules/uavcan/module.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index e3fd31a9a1a7..00f4acfa94f5 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp \ # # libuavcan # -include $(PX4_LIB_DIR)uavcan/libuavcan/include.mk +include $(UAVCAN_DIR)/libuavcan/include.mk # Use the relitive path to keep the genrated files in the BUILD_DIR SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_INC) @@ -69,7 +69,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV # # libuavcan drivers for STM32 # -include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/stm32/driver/include.mk +include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk # Use the relitive path to keep the genrated files in the BUILD_DIR SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_STM32_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) From 2e6fb9d47e57dab057049ed00b4476bd426caea2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 31 May 2015 14:43:21 +1000 Subject: [PATCH 121/293] uORB: removed generated esc_status.h --- src/modules/uORB/topics/esc_status.h | 116 --------------------------- 1 file changed, 116 deletions(-) delete mode 100644 src/modules/uORB/topics/esc_status.h diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h deleted file mode 100644 index 73fe0f9361f1..000000000000 --- a/src/modules/uORB/topics/esc_status.h +++ /dev/null @@ -1,116 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: @author Marco Bauer - * - * 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 esc_status.h - * Definition of the esc_status uORB topic. - * - * Published the state machine and the system status bitfields - * (see SYS_STATUS mavlink message), published only by commander app. - * - * All apps should write to subsystem_info: - * - * (any app) --> subsystem_info (published) --> (commander app state machine) --> esc_status --> (mavlink app) - */ - -#ifndef ESC_STATUS_H_ -#define ESC_STATUS_H_ - -#include -#include -#include "../uORB.h" - -/** - * The number of ESCs supported. - * Current (Q2/2013) we support 8 ESCs, - */ -#define CONNECTED_ESC_MAX 8 - -enum ESC_VENDOR { - ESC_VENDOR_GENERIC = 0, /**< generic ESC */ - ESC_VENDOR_MIKROKOPTER, /**< Mikrokopter */ - ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ -}; - -enum ESC_CONNECTION_TYPE { - ESC_CONNECTION_TYPE_PPM = 0, /**< Traditional PPM ESC */ - ESC_CONNECTION_TYPE_SERIAL, /**< Serial Bus connected ESC */ - ESC_CONNECTION_TYPE_ONESHOOT, /**< One Shoot PPM */ - ESC_CONNECTION_TYPE_I2C, /**< I2C */ - ESC_CONNECTION_TYPE_CAN /**< CAN-Bus */ -}; - -/** - * @addtogroup topics - * @{ - */ - -/** - * Electronic speed controller status. - * Unsupported float fields should be assigned NaN. - */ -struct esc_status_s { - /* use of a counter and timestamp recommended (but not necessary) */ - - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - uint8_t esc_count; /**< number of connected ESCs */ - enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */ - - struct { - enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ - uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ - int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ - float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ - float esc_current; /**< Current measured from current ESC [A] - if supported */ - float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ - float esc_setpoint; /**< setpoint of current ESC */ - uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ - uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ - uint16_t esc_version; /**< Version of current ESC - if supported */ - uint16_t esc_state; /**< State of ESC - depend on Vendor */ - } esc[CONNECTED_ESC_MAX]; - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -//ORB_DECLARE(esc_status); -ORB_DECLARE_OPTIONAL(esc_status); - -#endif From 3aa83a9b396f41258dcd330fc2ef796bcc2b6146 Mon Sep 17 00:00:00 2001 From: Michael Landes Date: Mon, 17 Nov 2014 00:04:45 -0500 Subject: [PATCH 122/293] irlock: initial version of IR-LOCK sensor driver Also works with the Pixy Cam --- src/drivers/drv_irlock.h | 65 +++++ src/drivers/irlock/irlock.cpp | 499 ++++++++++++++++++++++++++++++++++ src/drivers/irlock/module.mk | 42 +++ 3 files changed, 606 insertions(+) create mode 100644 src/drivers/drv_irlock.h create mode 100644 src/drivers/irlock/irlock.cpp create mode 100644 src/drivers/irlock/module.mk diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h new file mode 100644 index 000000000000..612bf9464a57 --- /dev/null +++ b/src/drivers/drv_irlock.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/* + * drv_irlock.h + * + * Created on: Nov 12, 2014 + * Author: MLandes + */ + +#ifndef _DRV_IRLOCK_H +#define _DRV_IRLOCK_H + +#include +#include + +#include "drv_sensor.h" // include sensor driver interfaces +#include "drv_orb_dev.h" + +#define IRLOCK_DEVICE_PATH "/dev/irlock" + +/* + * ObjDev tag for irlock data. + */ +ORB_DECLARE(irlock_data); + +/* + * ioctl() definitions + * + * custom irlock sensor driver interface (none currently) + */ +#define _IRLOCKIOCBASE (0x7800) +#define __IRLOCKIOC(_n) (_IOC(_IRLOCKIOCBASE, _n)) + +#endif /* _DRV_IRLOCK_H */ diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp new file mode 100644 index 000000000000..45aab2cdc19a --- /dev/null +++ b/src/drivers/irlock/irlock.cpp @@ -0,0 +1,499 @@ +/**************************************************************************** + * + * 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 irlock.cpp + * @author Michael Landes + * + * Driver for the IRLock sensor connected via I2C. + * + * Created on: Nov 12, 2014 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "drivers/boards/px4fmu-v2/board_config.h" +#include "drivers/device/i2c.h" +#include "drivers/device/ringbuffer.h" +#include "drivers/drv_irlock.h" +#include "drivers/drv_hrt.h" + +#include "nuttx/clock.h" +#include "nuttx/wqueue.h" +#include "systemlib/err.h" + +#include "uORB/topics/irlock.h" +#include "uORB/topics/subsystem_info.h" +#include "uORB/uORB.h" + +/* Configuration Constants */ +#define IRLOCK_BUS PX4_I2C_BUS_EXPANSION +#define I2C_IRLOCK_ADDRESS 0x65 //* 7-bit address (non shifted) +#define IRLOCK_CONVERSION_INTERVAL 20000 /* us = 20ms = 50Hz */ + +#define IRLOCK_SYNC 0xAA55 +#define IRLOCK_RESYNC 0x5500 +#define IRLOCK_ADJUST 0xAA + +/* 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 IRLOCK : public device::I2C +{ +public: + IRLOCK(int bus = IRLOCK_BUS, int address = I2C_IRLOCK_ADDRESS); + virtual ~IRLOCK(); + + virtual int init(); + + void print_diagnostics(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + +protected: + virtual int probe(); + +private: + // static callback for the work queue, arg will be pointer to instance of this class + static void cycle_trampoline(void *arg); + + RingBuffer *_reports; + orb_advert_t _irlock_topic; + bool _sensor_ok; + int _measure_ticks; + work_s _work; + + // irlock device io + int read_device(); + bool sync_device(); + int read_device_word(uint16_t *word); + int read_device_block(struct irlock_s *block); + + // driver control + void start(); + void stop(); + void cycle(); +}; + +extern "C" __EXPORT int irlock_main(int argc, char *argv[]); + +IRLOCK::IRLOCK(int bus, int address) : + I2C("IRLOCK", IRLOCK_DEVICE_PATH, bus, address, 400000), // 400 khz - supposedly pointless number... + _reports(nullptr), + _irlock_topic(-1), + _sensor_ok(false), + _measure_ticks(0) +{ + memset(&_work, 0, sizeof(_work)); +} + +IRLOCK::~IRLOCK() +{ + stop(); + + if (_reports != nullptr) { + delete _reports; + } +} + +int IRLOCK::init() +{ + int status = ERROR; + + if (I2C::init() != OK) { + goto finish; + } + + _reports = new RingBuffer(135, sizeof(struct irlock_s)); + if (_reports == nullptr) + goto finish; + + status = OK; + _sensor_ok = true; + +finish: + return status; +} + +bool IRLOCK::sync_device() +{ + uint8_t sync_byte; + uint16_t sync_word; + + if (read_device_word(&sync_word) != OK) + return false; + + if (sync_word == IRLOCK_RESYNC) { + transfer(nullptr, 0, &sync_byte, 1); + if (sync_byte == IRLOCK_ADJUST) + return true; + } else if (sync_word == IRLOCK_SYNC) { + return true; + } + + return false; +} + +int IRLOCK::probe() +{ + /* + * IRLock defaults to sending 0x00 when there is no block + * data to return, so really all we can do is check to make + * sure a transfer completes successfully. + */ + uint8_t byte; + if (transfer(nullptr, 0, &byte, 1) != OK) { + return -EIO; + } + + return OK; +} + +int IRLOCK::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch(cmd) { + case SENSORIOCSPOLLRATE: // set poll rate (and start polling) + switch (arg) { + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + bool stopped = (_measure_ticks == 0); + + _measure_ticks = USEC2TICK(IRLOCK_CONVERSION_INTERVAL); + if (stopped) { + start(); + } + + return OK; + } + // not yet supporting manual, custom or external polling + case SENSOR_POLLRATE_MANUAL: + warnx("manual polling not currently supported"); + return -EINVAL; + case SENSOR_POLLRATE_EXTERNAL: + warnx("external polling not currently supported"); + return -EINVAL; + default: + warnx("custom poll-rates not currently supported"); + return -EINVAL; + } + warnx("set poll rate"); + return OK; + case SENSORIOCRESET: + warnx("reset not implemented, use stop and then start"); + return -EINVAL; + default: + warnx("default ioctl call"); + return OK; // I2C::ioctl(filp, cmd, arg); - causes errors for some reason + } +} + +ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct irlock_s); + struct irlock_s *rbuf = reinterpret_cast(buffer); + int ret = 0; + + if (count < 1) + return -ENOSPC; + + // only try to read if we are using automatic polling in the driver + if (_measure_ticks > 0) { + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + ++rbuf; + } + } + + return ret ? ret : -EAGAIN; + } + + return ret; +} + +int IRLOCK::read_device() +{ + // if we sync, then we are starting a new frame, else fail + if (!sync_device()) + return ERROR; + + // now read blocks until sync stops, first flush stale queue data + _reports->flush(); + while (sync_device()) { + struct irlock_s block; + if (read_device_block(&block) != OK) + break; + + _reports->force(&block); + } + + return OK; +} + +int IRLOCK::read_device_word(uint16_t *word) +{ + uint8_t bytes[2]; + memset(bytes, 0, sizeof bytes); + + int status = transfer(nullptr, 0, &bytes[0], 2); + *word = bytes[1] << 8 | bytes[0]; + + return status; +} + +int IRLOCK::read_device_block(struct irlock_s *block) +{ + uint8_t bytes[12]; + memset(bytes, 0, sizeof bytes); + + int status = transfer(nullptr, 0, &bytes[0], 12); + uint16_t checksum = bytes[1] << 8 | bytes[0]; + block->signature = bytes[3] << 8 | bytes[2]; + block->center_x = bytes[5] << 8 | bytes[4]; + block->center_y = bytes[7] << 8 | bytes[6]; + block->width = bytes[9] << 8 | bytes[8]; + block->height = bytes[11] << 8 | bytes[10]; + + if (block->signature + block->center_x + block->center_y + block->width + block->height != checksum) + return -EIO; + + block->timestamp = hrt_absolute_time(); + return status; +} + +void IRLOCK::start() +{ + // flush ring and reset state machine + _reports->flush(); + + // start work queue cycle + work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, 1); +} + +void IRLOCK::stop() +{ + warnx("stopping queue work"); + work_cancel(HPWORK, &_work); +} + +void IRLOCK::cycle_trampoline(void *arg) +{ + IRLOCK *device = (IRLOCK*)arg; + device->cycle(); +} + +void IRLOCK::cycle() +{ + // ignoring failure, if we do, we will be back again right away... + read_device(); + + // schedule the next cycle + work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, USEC2TICK(IRLOCK_CONVERSION_INTERVAL)); +} + +void IRLOCK::print_diagnostics() +{ + if (!_sensor_ok) + warnx("sensor is not ok"); + + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue: "); +} + +namespace irlock +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +IRLOCK *g_dev = nullptr; + +void start(); +void stop(); +void test(); +void reset(); +void info(); +void usage(); + +void start() +{ + int fd; + + if (g_dev != nullptr) { + warnx("irlock device driver has already been started"); + exit(OK); + } + + g_dev = new IRLOCK(IRLOCK_BUS, I2C_IRLOCK_ADDRESS); + if (g_dev == nullptr) { + warnx("failed to instantiate device reference"); + goto fail; + } + + if (g_dev->init() != OK) { + warnx("failed to initialize device"); + goto fail; + } + + fd = open(IRLOCK_DEVICE_PATH, O_RDONLY); + if (fd < 0) { + warnx("could not open device file"); + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { + warnx("set poll rate on device failed"); + goto fail; + } + + exit(OK); + +fail: + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + exit(ERROR); +} + +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } else { + warnx("irlock device driver was not running"); + } + + exit(OK); +} + +void reset() +{ + int fd = open(IRLOCK_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + errx(ERROR, "driver access failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + errx(ERROR, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + errx(ERROR, "driver poll restart failed"); + + exit(OK); +} + +void test() +{ + errx(1, "test - not implmented"); +} + +void info() +{ + if (g_dev == nullptr) { + errx(1, "irlock device driver is not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_diagnostics(); + + exit(0); +} + +void usage() +{ + warnx("missing command: try 'start', 'stop', 'reset', 'test', 'info'"); +} + +} // namespace irlock + +int irlock_main(int argc, char *argv[]) +{ + const char *command = argv[1]; + + /* + * Start/load the driver + */ + if (!strcmp(command, "start")) { + irlock::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(command, "stop")) { + irlock::stop(); + } + + /* + * Reset the driver + */ + if (!strcmp(command, "reset")) { + irlock::reset(); + } + + /* + * Test the driver/device + */ + if (!strcmp(command, "test")) { + irlock::test(); + } + + /* + * Print driver information + */ + if (!strcmp(command, "info") || !strcmp(command, "status")) { + irlock::info(); + } + + irlock::usage(); +} diff --git a/src/drivers/irlock/module.mk b/src/drivers/irlock/module.mk new file mode 100644 index 000000000000..da3e1e42a1f2 --- /dev/null +++ b/src/drivers/irlock/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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 IRLock driver. +# + +MODULE_COMMAND = irlock + +SRCS = irlock.cpp + +MAXOPTIMIZATION = -Os From 5ebef78221fe07bcbae2aeec729c31f9f175249d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 1 Mar 2015 16:32:58 +0900 Subject: [PATCH 123/293] irlock: simplify driver Remove ioctl calls by always starting cycling Remove unused orb variables and includes Remove unused angle from irlock_s structure Add test and set I2C address to pixy default Reduce max num objects to 5 Add read errors reporting via nsh --- src/drivers/drv_irlock.h | 34 +-- src/drivers/irlock/irlock.cpp | 529 ++++++++++++++++------------------ 2 files changed, 263 insertions(+), 300 deletions(-) diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h index 612bf9464a57..9f1487a311a0 100644 --- a/src/drivers/drv_irlock.h +++ b/src/drivers/drv_irlock.h @@ -32,34 +32,30 @@ ****************************************************************************/ /* - * drv_irlock.h + * @file drv_irlock.h * - * Created on: Nov 12, 2014 - * Author: MLandes + * IR-Lock device API */ -#ifndef _DRV_IRLOCK_H -#define _DRV_IRLOCK_H +#pragma once #include #include #include "drv_sensor.h" // include sensor driver interfaces -#include "drv_orb_dev.h" -#define IRLOCK_DEVICE_PATH "/dev/irlock" +#define IRLOCK_BASE_DEVICE_PATH "/dev/irlock" +#define IRLOCK0_DEVICE_PATH "/dev/irlock0" -/* - * ObjDev tag for irlock data. - */ -ORB_DECLARE(irlock_data); +#define IRLOCK_OBJECTS_MAX 5 /* up to 5 objects can be detected/reported */ -/* - * ioctl() definitions - * - * custom irlock sensor driver interface (none currently) - */ -#define _IRLOCKIOCBASE (0x7800) -#define __IRLOCKIOC(_n) (_IOC(_IRLOCKIOCBASE, _n)) +/* irlock_s structure returned from read calls */ +struct irlock_s { + uint64_t timestamp; // microseconds since system start -#endif /* _DRV_IRLOCK_H */ + uint16_t signature; + uint16_t center_x; + uint16_t center_y; + uint16_t width; + uint16_t height; +}; diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 45aab2cdc19a..8ccffc426a5c 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -35,9 +35,9 @@ * @file irlock.cpp * @author Michael Landes * - * Driver for the IRLock sensor connected via I2C. + * Driver for an IR-Lock and Pixy vision sensor connected via I2C. * - * Created on: Nov 12, 2014 + * Created on: Nov 12, 2014 */ #include @@ -48,35 +48,25 @@ #include #include -#include "drivers/boards/px4fmu-v2/board_config.h" -#include "drivers/device/i2c.h" -#include "drivers/device/ringbuffer.h" -#include "drivers/drv_irlock.h" -#include "drivers/drv_hrt.h" +#include +#include +#include +#include +#include -#include "nuttx/clock.h" -#include "nuttx/wqueue.h" -#include "systemlib/err.h" - -#include "uORB/topics/irlock.h" -#include "uORB/topics/subsystem_info.h" -#include "uORB/uORB.h" +#include +#include +#include /* Configuration Constants */ -#define IRLOCK_BUS PX4_I2C_BUS_EXPANSION -#define I2C_IRLOCK_ADDRESS 0x65 //* 7-bit address (non shifted) -#define IRLOCK_CONVERSION_INTERVAL 20000 /* us = 20ms = 50Hz */ +#define IRLOCK_I2C_BUS PX4_I2C_BUS_EXPANSION +#define IRLOCK_I2C_ADDRESS 0x54 //* 7-bit address (non shifted) +#define IRLOCK_CONVERSION_INTERVAL_US 20000U /* us = 20ms = 50Hz */ #define IRLOCK_SYNC 0xAA55 #define IRLOCK_RESYNC 0x5500 #define IRLOCK_ADJUST 0xAA -/* 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 @@ -84,100 +74,99 @@ static const int ERROR = -1; class IRLOCK : public device::I2C { public: - IRLOCK(int bus = IRLOCK_BUS, int address = I2C_IRLOCK_ADDRESS); + IRLOCK(int bus = IRLOCK_I2C_BUS, int address = IRLOCK_I2C_ADDRESS); virtual ~IRLOCK(); virtual int init(); + virtual int probe(); + virtual int info(); + virtual int test(); - void print_diagnostics(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); -protected: - virtual int probe(); - private: - // static callback for the work queue, arg will be pointer to instance of this class - static void cycle_trampoline(void *arg); - RingBuffer *_reports; - orb_advert_t _irlock_topic; - bool _sensor_ok; - int _measure_ticks; - work_s _work; + /* start periodic reads from sensor */ + void start(); + + /* stop periodic reads from sensor */ + void stop(); - // irlock device io + /* static function that is called by worker queue, arg will be pointer to instance of this class */ + static void cycle_trampoline(void *arg); + + /* read from device and schedule next read */ + void cycle(); \ + + /* low level communication with sensor */ int read_device(); bool sync_device(); int read_device_word(uint16_t *word); int read_device_block(struct irlock_s *block); - // driver control - void start(); - void stop(); - void cycle(); + /* internal variables */ + RingBuffer *_reports; + bool _sensor_ok; + work_s _work; + uint32_t _read_failures; }; +/* global pointer for single IRLOCK sensor */ +namespace +{ +IRLOCK *g_irlock = nullptr; +} + +void irlock_usage(); + extern "C" __EXPORT int irlock_main(int argc, char *argv[]); +/* constructor */ IRLOCK::IRLOCK(int bus, int address) : - I2C("IRLOCK", IRLOCK_DEVICE_PATH, bus, address, 400000), // 400 khz - supposedly pointless number... - _reports(nullptr), - _irlock_topic(-1), - _sensor_ok(false), - _measure_ticks(0) + I2C("irlock", IRLOCK0_DEVICE_PATH, bus, address, 400000), + _reports(nullptr), + _sensor_ok(false), + _read_failures(0) { memset(&_work, 0, sizeof(_work)); } +/* destructor */ IRLOCK::~IRLOCK() { stop(); + /* clear reports queue */ if (_reports != nullptr) { delete _reports; } } +/* initialise driver to communicate with sensor */ int IRLOCK::init() { - int status = ERROR; + /* initialise I2C bus */ + int ret = I2C::init(); - if (I2C::init() != OK) { - goto finish; + if (ret != OK) { + return ret; } - _reports = new RingBuffer(135, sizeof(struct irlock_s)); - if (_reports == nullptr) - goto finish; - - status = OK; - _sensor_ok = true; + /* allocate buffer storing values read from sensor */ + _reports = new RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); -finish: - return status; -} - -bool IRLOCK::sync_device() -{ - uint8_t sync_byte; - uint16_t sync_word; + if (_reports == nullptr) { + return ENOTTY; - if (read_device_word(&sync_word) != OK) - return false; - - if (sync_word == IRLOCK_RESYNC) { - transfer(nullptr, 0, &sync_byte, 1); - if (sync_byte == IRLOCK_ADJUST) - return true; - } else if (sync_word == IRLOCK_SYNC) { - return true; + } else { + _sensor_ok = true; + /* start work queue */ + start(); + return OK; } - - return false; } +/* probe the device is on the I2C bus */ int IRLOCK::probe() { /* @@ -186,6 +175,7 @@ int IRLOCK::probe() * sure a transfer completes successfully. */ uint8_t byte; + if (transfer(nullptr, 0, &byte, 1) != OK) { return -EIO; } @@ -193,118 +183,66 @@ int IRLOCK::probe() return OK; } -int IRLOCK::ioctl(struct file *filp, int cmd, unsigned long arg) +/* display driver info */ +int IRLOCK::info() { - switch(cmd) { - case SENSORIOCSPOLLRATE: // set poll rate (and start polling) - switch (arg) { - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - bool stopped = (_measure_ticks == 0); - - _measure_ticks = USEC2TICK(IRLOCK_CONVERSION_INTERVAL); - if (stopped) { - start(); - } - - return OK; - } - // not yet supporting manual, custom or external polling - case SENSOR_POLLRATE_MANUAL: - warnx("manual polling not currently supported"); - return -EINVAL; - case SENSOR_POLLRATE_EXTERNAL: - warnx("external polling not currently supported"); - return -EINVAL; - default: - warnx("custom poll-rates not currently supported"); - return -EINVAL; - } - warnx("set poll rate"); - return OK; - case SENSORIOCRESET: - warnx("reset not implemented, use stop and then start"); - return -EINVAL; - default: - warnx("default ioctl call"); - return OK; // I2C::ioctl(filp, cmd, arg); - causes errors for some reason + if (g_irlock == nullptr) { + errx(1, "irlock device driver is not running"); } -} - -ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct irlock_s); - struct irlock_s *rbuf = reinterpret_cast(buffer); - int ret = 0; - if (count < 1) - return -ENOSPC; + /* display reports in queue */ + if (_sensor_ok) { + _reports->print_info("report queue: "); + warnx("read errors:%lu", (unsigned long)_read_failures); - // only try to read if we are using automatic polling in the driver - if (_measure_ticks > 0) { - while (count--) { - if (_reports->get(rbuf)) { - ret += sizeof(*rbuf); - ++rbuf; - } - } - - return ret ? ret : -EAGAIN; + } else { + warnx("sensor is not healthy"); } - return ret; + return OK; } -int IRLOCK::read_device() +/* test driver */ +int IRLOCK::test() { - // if we sync, then we are starting a new frame, else fail - if (!sync_device()) - return ERROR; - - // now read blocks until sync stops, first flush stale queue data - _reports->flush(); - while (sync_device()) { - struct irlock_s block; - if (read_device_block(&block) != OK) - break; - - _reports->force(&block); + /* exit immediately if driver not running */ + if (g_irlock == nullptr) { + errx(1, "irlock device driver is not running"); } - return OK; -} - -int IRLOCK::read_device_word(uint16_t *word) -{ - uint8_t bytes[2]; - memset(bytes, 0, sizeof bytes); + /* exit immediately if sensor is not healty */ + if (!_sensor_ok) { + errx(1, "sensor is not healthy"); + } - int status = transfer(nullptr, 0, &bytes[0], 2); - *word = bytes[1] << 8 | bytes[0]; + /* instructions to user */ + warnx("searching for object for 10 seconds"); - return status; -} + /* read from sensor for 10 seconds */ + struct irlock_s obj_report; + uint64_t start_time = hrt_absolute_time(); -int IRLOCK::read_device_block(struct irlock_s *block) -{ - uint8_t bytes[12]; - memset(bytes, 0, sizeof bytes); + while ((hrt_absolute_time() - start_time) < 10000000) { - int status = transfer(nullptr, 0, &bytes[0], 12); - uint16_t checksum = bytes[1] << 8 | bytes[0]; - block->signature = bytes[3] << 8 | bytes[2]; - block->center_x = bytes[5] << 8 | bytes[4]; - block->center_y = bytes[7] << 8 | bytes[6]; - block->width = bytes[9] << 8 | bytes[8]; - block->height = bytes[11] << 8 | bytes[10]; + /* output all objects found */ + while (_reports->count() > 0) { + _reports->get(&obj_report); + warnx("sig:%d x:%d y:%d width:%d height:%d", + (int)obj_report.signature, + (int)obj_report.center_x, + (int)obj_report.center_y, + (int)obj_report.width, + (int)obj_report.height); + } - if (block->signature + block->center_x + block->center_y + block->width + block->height != checksum) - return -EIO; + /* sleep for 0.05 seconds */ + usleep(50000); + } - block->timestamp = hrt_absolute_time(); - return status; + return OK; } +/* start periodic reads from sensor */ void IRLOCK::start() { // flush ring and reset state machine @@ -314,16 +252,20 @@ void IRLOCK::start() work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, 1); } +/* stop periodic reads from sensor */ void IRLOCK::stop() { - warnx("stopping queue work"); work_cancel(HPWORK, &_work); } void IRLOCK::cycle_trampoline(void *arg) { - IRLOCK *device = (IRLOCK*)arg; - device->cycle(); + IRLOCK *device = (IRLOCK *)arg; + + /* check global irlock reference and cycle */ + if (g_irlock != nullptr) { + device->cycle(); + } } void IRLOCK::cycle() @@ -332,168 +274,193 @@ void IRLOCK::cycle() read_device(); // schedule the next cycle - work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, USEC2TICK(IRLOCK_CONVERSION_INTERVAL)); + work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, USEC2TICK(IRLOCK_CONVERSION_INTERVAL_US)); } -void IRLOCK::print_diagnostics() +ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen) { - if (!_sensor_ok) - warnx("sensor is not ok"); - - printf("poll interval: %u ticks\n", _measure_ticks); - _reports->print_info("report queue: "); -} + unsigned count = buflen / sizeof(struct irlock_s); + struct irlock_s *rbuf = reinterpret_cast(buffer); + int ret = 0; -namespace irlock -{ + if (count < 1) { + return -ENOSPC; + } -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -const int ERROR = -1; + // try to read + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + ++rbuf; + } + } -IRLOCK *g_dev = nullptr; + return ret ? ret : -EAGAIN; -void start(); -void stop(); -void test(); -void reset(); -void info(); -void usage(); + return ret; +} -void start() +/* sync device to ensure reading starts at new frame*/ +bool IRLOCK::sync_device() { - int fd; + uint8_t sync_byte; + uint16_t sync_word; - if (g_dev != nullptr) { - warnx("irlock device driver has already been started"); - exit(OK); + if (read_device_word(&sync_word) != OK) { + return false; } - g_dev = new IRLOCK(IRLOCK_BUS, I2C_IRLOCK_ADDRESS); - if (g_dev == nullptr) { - warnx("failed to instantiate device reference"); - goto fail; - } + if (sync_word == IRLOCK_RESYNC) { + transfer(nullptr, 0, &sync_byte, 1); - if (g_dev->init() != OK) { - warnx("failed to initialize device"); - goto fail; - } + if (sync_byte == IRLOCK_ADJUST) { + return true; + } - fd = open(IRLOCK_DEVICE_PATH, O_RDONLY); - if (fd < 0) { - warnx("could not open device file"); - goto fail; + } else if (sync_word == IRLOCK_SYNC) { + return true; } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { - warnx("set poll rate on device failed"); - goto fail; + return false; +} + +/* read all available frames from sensor */ +int IRLOCK::read_device() +{ + // if we sync, then we are starting a new frame, else fail + if (!sync_device()) { + return -ENOTTY; } - exit(OK); + // now read blocks until sync stops, first flush stale queue data + _reports->flush(); + int num_objects = 0; + + while (sync_device() && (num_objects < IRLOCK_OBJECTS_MAX)) { + struct irlock_s block; -fail: - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (read_device_block(&block) != OK) { + break; + } + + _reports->force(&block); } - exit(ERROR); + return OK; } -void stop() +/* read a word (two bytes) from sensor */ +int IRLOCK::read_device_word(uint16_t *word) { - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } else { - warnx("irlock device driver was not running"); - } + uint8_t bytes[2]; + memset(bytes, 0, sizeof bytes); + + int status = transfer(nullptr, 0, &bytes[0], 2); + *word = bytes[1] << 8 | bytes[0]; - exit(OK); + return status; } -void reset() +/* read a single block (a full frame) from sensor */ +int IRLOCK::read_device_block(struct irlock_s *block) { - int fd = open(IRLOCK_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - errx(ERROR, "driver access failed "); + uint8_t bytes[12]; + memset(bytes, 0, sizeof bytes); - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - errx(ERROR, "driver reset failed"); + int status = transfer(nullptr, 0, &bytes[0], 12); + uint16_t checksum = bytes[1] << 8 | bytes[0]; + block->signature = bytes[3] << 8 | bytes[2]; + block->center_x = bytes[5] << 8 | bytes[4]; + block->center_y = bytes[7] << 8 | bytes[6]; + block->width = bytes[9] << 8 | bytes[8]; + block->height = bytes[11] << 8 | bytes[10]; - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - errx(ERROR, "driver poll restart failed"); + // crc check + if (block->signature + block->center_x + block->center_y + block->width + block->height != checksum) { + _read_failures++; + return -EIO; + } - exit(OK); + block->timestamp = hrt_absolute_time(); + return status; } -void test() +void irlock_usage() { - errx(1, "test - not implmented"); + warnx("missing command: try 'start', 'stop', 'info', 'test'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", IRLOCK_I2C_BUS); } -void info() +int irlock_main(int argc, char *argv[]) { - if (g_dev == nullptr) { - errx(1, "irlock device driver is not running"); + int i2cdevice = IRLOCK_I2C_BUS; + + /* jump over start/off/etc and look at options first */ + if (getopt(argc, argv, "b:") != EOF) { + i2cdevice = (int)strtol(optarg, NULL, 0); } - printf("state @ %p\n", g_dev); - g_dev->print_diagnostics(); + if (optind >= argc) { + irlock_usage(); + exit(1); + } - exit(0); -} + const char *command = argv[optind]; -void usage() -{ - warnx("missing command: try 'start', 'stop', 'reset', 'test', 'info'"); -} + /* start driver */ + if (!strcmp(command, "start")) { + if (g_irlock != nullptr) { + errx(1, "driver has already been started"); + } -} // namespace irlock + /* instantiate global instance */ + g_irlock = new IRLOCK(i2cdevice, IRLOCK_I2C_ADDRESS); -int irlock_main(int argc, char *argv[]) -{ - const char *command = argv[1]; + if (g_irlock == nullptr) { + errx(1, "failed to allocated memory for driver"); + } - /* - * Start/load the driver - */ - if (!strcmp(command, "start")) { - irlock::start(); + /* initialise global instance */ + if (g_irlock->init() != OK) { + IRLOCK *tmp_irlock = g_irlock; + g_irlock = nullptr; + delete tmp_irlock; + errx(1, "failed to initialize device, stopping driver"); + } + + exit(0); } - /* - * Stop the driver - */ - if (!strcmp(command, "stop")) { - irlock::stop(); + /* need the driver past this point */ + if (g_irlock == nullptr) { + warnx("not started"); + irlock_usage(); + exit(1); } - /* - * Reset the driver - */ - if (!strcmp(command, "reset")) { - irlock::reset(); + /* stop the driver */ + if (!strcmp(command, "stop")) { + IRLOCK *tmp_irlock = g_irlock; + g_irlock = nullptr; + delete tmp_irlock; + warnx("irlock stopped"); + exit(OK); } - /* - * Test the driver/device - */ - if (!strcmp(command, "test")) { - irlock::test(); + /* Print driver information */ + if (!strcmp(command, "info")) { + g_irlock->info(); + exit(OK); } - /* - * Print driver information - */ - if (!strcmp(command, "info") || !strcmp(command, "status")) { - irlock::info(); + /* test driver */ + if (!strcmp(command, "test")) { + g_irlock->test(); + exit(OK); } - irlock::usage(); + /* display usage info */ + irlock_usage(); + exit(0); } From fc6fc499e5e497887950ac7dd1d756cb9b059735 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 25 May 2015 14:34:18 +0900 Subject: [PATCH 124/293] irlock: formatting fixes --- src/drivers/drv_irlock.h | 12 ++-- src/drivers/irlock/irlock.cpp | 126 +++++++++++++++++----------------- src/drivers/irlock/module.mk | 33 --------- 3 files changed, 69 insertions(+), 102 deletions(-) diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h index 9f1487a311a0..06cf5c864591 100644 --- a/src/drivers/drv_irlock.h +++ b/src/drivers/drv_irlock.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,11 +31,11 @@ * ****************************************************************************/ -/* +/** * @file drv_irlock.h * * IR-Lock device API - */ + **/ #pragma once @@ -47,11 +47,11 @@ #define IRLOCK_BASE_DEVICE_PATH "/dev/irlock" #define IRLOCK0_DEVICE_PATH "/dev/irlock0" -#define IRLOCK_OBJECTS_MAX 5 /* up to 5 objects can be detected/reported */ +#define IRLOCK_OBJECTS_MAX 5 /** up to 5 objects can be detected/reported **/ -/* irlock_s structure returned from read calls */ +/** irlock_s structure returned from read calls **/ struct irlock_s { - uint64_t timestamp; // microseconds since system start + uint64_t timestamp; /** microseconds since system start **/ uint16_t signature; uint16_t center_x; diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 8ccffc426a5c..d22d9a38e497 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,14 +31,14 @@ * ****************************************************************************/ -/* +/** * @file irlock.cpp * @author Michael Landes * * Driver for an IR-Lock and Pixy vision sensor connected via I2C. * * Created on: Nov 12, 2014 - */ + **/ #include #include @@ -58,10 +58,10 @@ #include #include -/* Configuration Constants */ +/** Configuration Constants **/ #define IRLOCK_I2C_BUS PX4_I2C_BUS_EXPANSION -#define IRLOCK_I2C_ADDRESS 0x54 //* 7-bit address (non shifted) -#define IRLOCK_CONVERSION_INTERVAL_US 20000U /* us = 20ms = 50Hz */ +#define IRLOCK_I2C_ADDRESS 0x54 /** 7-bit address (non shifted) **/ +#define IRLOCK_CONVERSION_INTERVAL_US 20000U /** us = 20ms = 50Hz **/ #define IRLOCK_SYNC 0xAA55 #define IRLOCK_RESYNC 0x5500 @@ -86,32 +86,32 @@ class IRLOCK : public device::I2C private: - /* start periodic reads from sensor */ - void start(); + /** start periodic reads from sensor **/ + void start(); - /* stop periodic reads from sensor */ - void stop(); + /** stop periodic reads from sensor **/ + void stop(); - /* static function that is called by worker queue, arg will be pointer to instance of this class */ - static void cycle_trampoline(void *arg); + /** static function that is called by worker queue, arg will be pointer to instance of this class **/ + static void cycle_trampoline(void *arg); - /* read from device and schedule next read */ - void cycle(); \ + /** read from device and schedule next read **/ + void cycle(); - /* low level communication with sensor */ - int read_device(); - bool sync_device(); - int read_device_word(uint16_t *word); - int read_device_block(struct irlock_s *block); + /** low level communication with sensor **/ + int read_device(); + bool sync_device(); + int read_device_word(uint16_t *word); + int read_device_block(struct irlock_s *block); - /* internal variables */ + /** internal variables **/ RingBuffer *_reports; bool _sensor_ok; work_s _work; uint32_t _read_failures; }; -/* global pointer for single IRLOCK sensor */ +/** global pointer for single IRLOCK sensor **/ namespace { IRLOCK *g_irlock = nullptr; @@ -121,7 +121,7 @@ void irlock_usage(); extern "C" __EXPORT int irlock_main(int argc, char *argv[]); -/* constructor */ +/** constructor **/ IRLOCK::IRLOCK(int bus, int address) : I2C("irlock", IRLOCK0_DEVICE_PATH, bus, address, 400000), _reports(nullptr), @@ -131,28 +131,28 @@ IRLOCK::IRLOCK(int bus, int address) : memset(&_work, 0, sizeof(_work)); } -/* destructor */ +/** destructor **/ IRLOCK::~IRLOCK() { stop(); - /* clear reports queue */ + /** clear reports queue **/ if (_reports != nullptr) { delete _reports; } } -/* initialise driver to communicate with sensor */ +/** initialise driver to communicate with sensor **/ int IRLOCK::init() { - /* initialise I2C bus */ + /** initialise I2C bus **/ int ret = I2C::init(); if (ret != OK) { return ret; } - /* allocate buffer storing values read from sensor */ + /** allocate buffer storing values read from sensor **/ _reports = new RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); if (_reports == nullptr) { @@ -160,20 +160,20 @@ int IRLOCK::init() } else { _sensor_ok = true; - /* start work queue */ + /** start work queue **/ start(); return OK; } } -/* probe the device is on the I2C bus */ +/** probe the device is on the I2C bus **/ int IRLOCK::probe() { /* * IRLock defaults to sending 0x00 when there is no block * data to return, so really all we can do is check to make * sure a transfer completes successfully. - */ + **/ uint8_t byte; if (transfer(nullptr, 0, &byte, 1) != OK) { @@ -183,14 +183,14 @@ int IRLOCK::probe() return OK; } -/* display driver info */ +/** display driver info **/ int IRLOCK::info() { if (g_irlock == nullptr) { errx(1, "irlock device driver is not running"); } - /* display reports in queue */ + /** display reports in queue **/ if (_sensor_ok) { _reports->print_info("report queue: "); warnx("read errors:%lu", (unsigned long)_read_failures); @@ -202,29 +202,29 @@ int IRLOCK::info() return OK; } -/* test driver */ +/** test driver **/ int IRLOCK::test() { - /* exit immediately if driver not running */ + /** exit immediately if driver not running **/ if (g_irlock == nullptr) { errx(1, "irlock device driver is not running"); } - /* exit immediately if sensor is not healty */ + /** exit immediately if sensor is not healty **/ if (!_sensor_ok) { errx(1, "sensor is not healthy"); } - /* instructions to user */ + /** instructions to user **/ warnx("searching for object for 10 seconds"); - /* read from sensor for 10 seconds */ + /** read from sensor for 10 seconds **/ struct irlock_s obj_report; uint64_t start_time = hrt_absolute_time(); while ((hrt_absolute_time() - start_time) < 10000000) { - /* output all objects found */ + /** output all objects found **/ while (_reports->count() > 0) { _reports->get(&obj_report); warnx("sig:%d x:%d y:%d width:%d height:%d", @@ -235,24 +235,24 @@ int IRLOCK::test() (int)obj_report.height); } - /* sleep for 0.05 seconds */ + /** sleep for 0.05 seconds **/ usleep(50000); } return OK; } -/* start periodic reads from sensor */ +/** start periodic reads from sensor **/ void IRLOCK::start() { - // flush ring and reset state machine + /** flush ring and reset state machine **/ _reports->flush(); - // start work queue cycle + /** start work queue cycle **/ work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, 1); } -/* stop periodic reads from sensor */ +/** stop periodic reads from sensor **/ void IRLOCK::stop() { work_cancel(HPWORK, &_work); @@ -262,7 +262,7 @@ void IRLOCK::cycle_trampoline(void *arg) { IRLOCK *device = (IRLOCK *)arg; - /* check global irlock reference and cycle */ + /** check global irlock reference and cycle **/ if (g_irlock != nullptr) { device->cycle(); } @@ -270,10 +270,10 @@ void IRLOCK::cycle_trampoline(void *arg) void IRLOCK::cycle() { - // ignoring failure, if we do, we will be back again right away... + /** ignoring failure, if we do, we will be back again right away... **/ read_device(); - // schedule the next cycle + /** schedule the next cycle **/ work_queue(HPWORK, &_work, (worker_t)&IRLOCK::cycle_trampoline, this, USEC2TICK(IRLOCK_CONVERSION_INTERVAL_US)); } @@ -287,7 +287,7 @@ ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; } - // try to read + /** try to read **/ while (count--) { if (_reports->get(rbuf)) { ret += sizeof(*rbuf); @@ -300,7 +300,7 @@ ssize_t IRLOCK::read(struct file *filp, char *buffer, size_t buflen) return ret; } -/* sync device to ensure reading starts at new frame*/ +/** sync device to ensure reading starts at new frame*/ bool IRLOCK::sync_device() { uint8_t sync_byte; @@ -324,15 +324,15 @@ bool IRLOCK::sync_device() return false; } -/* read all available frames from sensor */ +/** read all available frames from sensor **/ int IRLOCK::read_device() { - // if we sync, then we are starting a new frame, else fail + /** if we sync, then we are starting a new frame, else fail **/ if (!sync_device()) { return -ENOTTY; } - // now read blocks until sync stops, first flush stale queue data + /** now read blocks until sync stops, first flush stale queue data **/ _reports->flush(); int num_objects = 0; @@ -349,7 +349,7 @@ int IRLOCK::read_device() return OK; } -/* read a word (two bytes) from sensor */ +/** read a word (two bytes) from sensor **/ int IRLOCK::read_device_word(uint16_t *word) { uint8_t bytes[2]; @@ -361,7 +361,7 @@ int IRLOCK::read_device_word(uint16_t *word) return status; } -/* read a single block (a full frame) from sensor */ +/** read a single block (a full frame) from sensor **/ int IRLOCK::read_device_block(struct irlock_s *block) { uint8_t bytes[12]; @@ -375,7 +375,7 @@ int IRLOCK::read_device_block(struct irlock_s *block) block->width = bytes[9] << 8 | bytes[8]; block->height = bytes[11] << 8 | bytes[10]; - // crc check + /** crc check **/ if (block->signature + block->center_x + block->center_y + block->width + block->height != checksum) { _read_failures++; return -EIO; @@ -396,7 +396,7 @@ int irlock_main(int argc, char *argv[]) { int i2cdevice = IRLOCK_I2C_BUS; - /* jump over start/off/etc and look at options first */ + /** jump over start/off/etc and look at options first **/ if (getopt(argc, argv, "b:") != EOF) { i2cdevice = (int)strtol(optarg, NULL, 0); } @@ -408,20 +408,20 @@ int irlock_main(int argc, char *argv[]) const char *command = argv[optind]; - /* start driver */ + /** start driver **/ if (!strcmp(command, "start")) { if (g_irlock != nullptr) { errx(1, "driver has already been started"); } - /* instantiate global instance */ + /** instantiate global instance **/ g_irlock = new IRLOCK(i2cdevice, IRLOCK_I2C_ADDRESS); if (g_irlock == nullptr) { errx(1, "failed to allocated memory for driver"); } - /* initialise global instance */ + /** initialise global instance **/ if (g_irlock->init() != OK) { IRLOCK *tmp_irlock = g_irlock; g_irlock = nullptr; @@ -432,14 +432,14 @@ int irlock_main(int argc, char *argv[]) exit(0); } - /* need the driver past this point */ + /** need the driver past this point **/ if (g_irlock == nullptr) { warnx("not started"); irlock_usage(); exit(1); } - /* stop the driver */ + /** stop the driver **/ if (!strcmp(command, "stop")) { IRLOCK *tmp_irlock = g_irlock; g_irlock = nullptr; @@ -448,19 +448,19 @@ int irlock_main(int argc, char *argv[]) exit(OK); } - /* Print driver information */ + /** Print driver information **/ if (!strcmp(command, "info")) { g_irlock->info(); exit(OK); } - /* test driver */ + /** test driver **/ if (!strcmp(command, "test")) { g_irlock->test(); exit(OK); } - /* display usage info */ + /** display usage info **/ irlock_usage(); exit(0); } diff --git a/src/drivers/irlock/module.mk b/src/drivers/irlock/module.mk index da3e1e42a1f2..5d9fbf4539d5 100644 --- a/src/drivers/irlock/module.mk +++ b/src/drivers/irlock/module.mk @@ -1,36 +1,3 @@ -############################################################################ -# -# 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 IRLock driver. # From 75d6fee77d195b2672fee68355c73277de21988f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 May 2015 14:52:11 +0900 Subject: [PATCH 125/293] irlock: report target in radians --- src/drivers/drv_irlock.h | 11 +++++------ src/drivers/irlock/irlock.cpp | 36 +++++++++++++++++++++++------------ 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h index 06cf5c864591..fa6b3016cd1a 100644 --- a/src/drivers/drv_irlock.h +++ b/src/drivers/drv_irlock.h @@ -52,10 +52,9 @@ /** irlock_s structure returned from read calls **/ struct irlock_s { uint64_t timestamp; /** microseconds since system start **/ - - uint16_t signature; - uint16_t center_x; - uint16_t center_y; - uint16_t width; - uint16_t height; + uint16_t target_num; /** target number prioritised by size (largest is 0) **/ + float angle_x; /** x-axis angle in radians from center of image to center of target **/ + float angle_y; /** y-axis angle in radians from center of image to center of target **/ + float size_x; /** size in radians of target along x-axis **/ + float size_y; /** size in radians of target along y-axis **/ }; diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index d22d9a38e497..4aeb8c7497c5 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -67,6 +67,11 @@ #define IRLOCK_RESYNC 0x5500 #define IRLOCK_ADJUST 0xAA +#define IRLOCK_CENTER_X 159 // the x-axis center pixel position +#define IRLOCK_CENTER_Y 99 // the y-axis center pixel position +#define IRLOCK_PIXELS_PER_RADIAN_X 307.9075f // x-axis pixel to radian scaler assuming 60deg FOV on x-axis +#define IRLOCK_PIXELS_PER_RADIAN_Y 326.4713f // y-axis pixel to radian scaler assuming 35deg FOV on y-axis + #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif @@ -227,12 +232,12 @@ int IRLOCK::test() /** output all objects found **/ while (_reports->count() > 0) { _reports->get(&obj_report); - warnx("sig:%d x:%d y:%d width:%d height:%d", - (int)obj_report.signature, - (int)obj_report.center_x, - (int)obj_report.center_y, - (int)obj_report.width, - (int)obj_report.height); + warnx("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f", + (int)obj_report.target_num, + (double)obj_report.angle_x, + (double)obj_report.angle_y, + (double)obj_report.size_x, + (double)obj_report.size_y); } /** sleep for 0.05 seconds **/ @@ -369,18 +374,25 @@ int IRLOCK::read_device_block(struct irlock_s *block) int status = transfer(nullptr, 0, &bytes[0], 12); uint16_t checksum = bytes[1] << 8 | bytes[0]; - block->signature = bytes[3] << 8 | bytes[2]; - block->center_x = bytes[5] << 8 | bytes[4]; - block->center_y = bytes[7] << 8 | bytes[6]; - block->width = bytes[9] << 8 | bytes[8]; - block->height = bytes[11] << 8 | bytes[10]; + uint16_t target_num = bytes[3] << 8 | bytes[2]; + uint16_t pixel_x = bytes[5] << 8 | bytes[4]; + uint16_t pixel_y = bytes[7] << 8 | bytes[6]; + uint16_t pixel_size_x = bytes[9] << 8 | bytes[8]; + uint16_t pixel_size_y = bytes[11] << 8 | bytes[10]; /** crc check **/ - if (block->signature + block->center_x + block->center_y + block->width + block->height != checksum) { + if (target_num + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) { _read_failures++; return -EIO; } + /** convert to angles **/ + block->target_num = target_num; + block->angle_x = (((float)(pixel_x-IRLOCK_CENTER_X))/IRLOCK_PIXELS_PER_RADIAN_X); + block->angle_y = (((float)(pixel_y-IRLOCK_CENTER_Y))/IRLOCK_PIXELS_PER_RADIAN_Y); + block->size_x = pixel_size_x / IRLOCK_PIXELS_PER_RADIAN_X; + block->size_y = pixel_size_y / IRLOCK_PIXELS_PER_RADIAN_Y; + block->timestamp = hrt_absolute_time(); return status; } From f10500d57c495133ce33e15d0ca304bada713568 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Jun 2015 13:19:37 +1000 Subject: [PATCH 126/293] ArduPilot: removed .gitmodules we do not want recursive git submodules for ArduPilot --- .gitmodules | 18 ------------------ 1 file changed, 18 deletions(-) delete mode 100644 .gitmodules diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index c869803b7ad7..000000000000 --- a/.gitmodules +++ /dev/null @@ -1,18 +0,0 @@ -[submodule "mavlink/include/mavlink/v1.0"] - path = mavlink/include/mavlink/v1.0 - url = git://github.com/mavlink/c_library.git -[submodule "NuttX"] - path = NuttX - url = git://github.com/PX4/NuttX.git -[submodule "uavcan"] - path = src/lib/uavcan - url = git://github.com/pavel-kirienko/uavcan.git -[submodule "Tools/genmsg"] - path = Tools/genmsg - url = https://github.com/ros/genmsg.git -[submodule "Tools/gencpp"] - path = Tools/gencpp - url = https://github.com/ros/gencpp.git -[submodule "unittests/gtest"] - path = unittests/gtest - url = https://github.com/sjwilks/gtest.git From 18ea4c4bc1915dcda99f7106dc29162270d076a6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Jun 2015 13:25:14 +1000 Subject: [PATCH 127/293] ArduPilot: ignore generated file --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 611325444e97..2928e9eb7106 100644 --- a/.gitignore +++ b/.gitignore @@ -46,3 +46,4 @@ Firmware.zip unittests/build *.generated.h .vagrant +makefiles/config_px4fmu-v2_APM.mk From 2035fe04678c10dd9c01fd260893d5d3301ea0a9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Jun 2015 14:07:03 +1000 Subject: [PATCH 128/293] ArduPilot: removed directories for old submodules we don't use recursive submodules --- NuttX | 1 - Tools/gencpp | 1 - Tools/genmsg | 1 - mavlink/include/mavlink/v1.0 | 1 - src/lib/uavcan | 1 - unittests/gtest | 1 - 6 files changed, 6 deletions(-) delete mode 160000 NuttX delete mode 160000 Tools/gencpp delete mode 160000 Tools/genmsg delete mode 160000 mavlink/include/mavlink/v1.0 delete mode 160000 src/lib/uavcan delete mode 160000 unittests/gtest diff --git a/NuttX b/NuttX deleted file mode 160000 index a164ab1bcdb4..000000000000 --- a/NuttX +++ /dev/null @@ -1 +0,0 @@ -Subproject commit a164ab1bcdb454779fa3eef661cfa023a2acd17a diff --git a/Tools/gencpp b/Tools/gencpp deleted file mode 160000 index 26a86f04bcec..000000000000 --- a/Tools/gencpp +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 26a86f04bcec0018af6652b3ddd3f680e6e3fa2a diff --git a/Tools/genmsg b/Tools/genmsg deleted file mode 160000 index 72f0383f0e6a..000000000000 --- a/Tools/genmsg +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 72f0383f0e6a489214c51802ae12d6e271b1e454 diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 deleted file mode 160000 index 68a88824741a..000000000000 --- a/mavlink/include/mavlink/v1.0 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 68a88824741ac26838ec0c8c38d67b51dc84b799 diff --git a/src/lib/uavcan b/src/lib/uavcan deleted file mode 160000 index 7719f7692aba..000000000000 --- a/src/lib/uavcan +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 7719f7692aba67f01b6321773bb7be13f23d2f68 diff --git a/unittests/gtest b/unittests/gtest deleted file mode 160000 index cdef6e4ce097..000000000000 --- a/unittests/gtest +++ /dev/null @@ -1 +0,0 @@ -Subproject commit cdef6e4ce097f953445446e8da4cb8bb68478bc5 From f0bd9df7cb5577b0531a9d61063bc657fb65c483 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Jun 2015 20:56:42 +1000 Subject: [PATCH 129/293] ArduPilot: ignore APM makefile --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 6cb43b70d282..01f693adb03c 100644 --- a/.gitignore +++ b/.gitignore @@ -51,3 +51,4 @@ xcode src/platforms/posix/px4_messages/ src/platforms/qurt/px4_messages/ makefiles/config_px4fmu-v2_APM.mk +makefiles/nuttx/config_px4fmu-v2_APM.mk From 883a21981e998c30e147c024a31b7bde111f547b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 11 Jun 2015 12:17:20 +1000 Subject: [PATCH 130/293] px4io: don't call io_set_rc_config() if RC handling is disabled --- src/drivers/px4io/px4io.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 59e1824dc86e..4179b189737b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1038,8 +1038,10 @@ PX4IO::task_main() param_set(dsm_bind_param, &dsm_bind_val); } - /* re-upload RC input config as it may have changed */ - io_set_rc_config(); + if (!_rc_handling_disabled) { + /* re-upload RC input config as it may have changed */ + io_set_rc_config(); + } /* re-set the battery scaling */ int32_t voltage_scaling_val; From b38850afd64af46bc7efde178e53a880fd8c75d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 11 Jun 2015 13:28:26 +1000 Subject: [PATCH 131/293] px4io: handle non-PX4 parameter system ensure that key values are initialised correctly when not using the PX4 parameter system --- src/drivers/px4io/px4io.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4179b189737b..d7e1b56fb45c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -675,9 +675,11 @@ PX4IO::init() if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; - param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); - param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); - param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); + if (!_rc_handling_disabled) { + param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); + param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); + param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); + } /* * Check for IO flight state - if FMU was flagged to be in @@ -1026,7 +1028,7 @@ PX4IO::task_main() parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); - int32_t dsm_bind_val; + int32_t dsm_bind_val = -1; param_t dsm_bind_param; /* see if bind parameter has been set, and reset it to -1 */ @@ -1044,7 +1046,7 @@ PX4IO::task_main() } /* re-set the battery scaling */ - int32_t voltage_scaling_val; + int32_t voltage_scaling_val = 10000; param_t voltage_scaling_param; /* set battery voltage scaling */ From 0cf4cfdb42174dbfe004717406ad6c88eb6c0d62 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 11 Jun 2015 13:28:59 +1000 Subject: [PATCH 132/293] systemlib: initialise value in circuit_breaker_enabled() this removes a reliance on the PX4 parameter system --- src/modules/systemlib/circuit_breaker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index ba9e9f62197c..5d9c7ec3716e 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -48,7 +48,7 @@ bool circuit_breaker_enabled(const char *breaker, int32_t magic) { - int32_t val; + int32_t val = 0; (void)PX4_PARAM_GET_BYNAME(breaker, &val); return (val == magic); From ed6ed1a1c452d8b95d7e460e192380aeeadd2110 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 11 Jun 2015 13:29:17 +1000 Subject: [PATCH 133/293] systemlib: don't include PX4 param system in ArduPilot build --- src/modules/systemlib/module.mk | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 54f76cda39fe..a205753c4322 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -37,13 +37,11 @@ SRCS = \ perf_counter.c \ - param/param.c \ conversions.c \ cpuload.c \ getopt_long.c \ pid/pid.c \ airspeed.c \ - system_params.c \ mavlink_log.c \ rc_check.c \ otp.c \ @@ -52,9 +50,15 @@ SRCS = \ mcu_version.c \ bson/tinybson.c \ circuit_breaker.cpp \ - circuit_breaker_params.c \ $(BUILD_DIR)git_version.c +ifneq ($(ARDUPILOT_BUILD),1) +# ArduPilot uses its own parameter system +SRCS += param/param.c \ + system_params.c \ + circuit_breaker_params.c +endif + ifeq ($(PX4_TARGET_OS),nuttx) SRCS += err.c \ up_cxxinitialize.c From 989af1c4c52d419af867b667012e7befdd76796a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 17 Jun 2015 17:03:46 +1000 Subject: [PATCH 134/293] perf: fixed perf to write to stdout --- src/systemcmds/perf/perf.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c index fb4e5030fc27..49551aec93c8 100644 --- a/src/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -70,7 +70,7 @@ int perf_main(int argc, char *argv[]) return 0; } else if (strcmp(argv[1], "latency") == 0) { - perf_print_latency(0 /* stdout */); + perf_print_latency(1 /* stdout */); fflush(stdout); return 0; } @@ -79,7 +79,7 @@ int perf_main(int argc, char *argv[]) return -1; } - perf_print_all(0 /* stdout */); + perf_print_all(1 /* stdout */); fflush(stdout); return 0; } From 8008f1890466fc648a02f2c739f1cefa7e6b0f40 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 6 Apr 2015 18:22:32 -0700 Subject: [PATCH 135/293] px4io: return failure when FORCE_SAFETY_OFF fails --- src/modules/px4iofirmware/registers.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 4ad7ba318294..6acdf6774817 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -608,12 +608,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_FORCE_SAFETY_ON: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } else { + return -1; } break; case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } else { + return -1; } break; From d5396ac1617e03e26170b020e9318f6fc727b3d2 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Tue, 31 Mar 2015 10:17:29 -0700 Subject: [PATCH 136/293] Added two commands for bootloader support --- src/drivers/drv_oreoled.h | 10 ++++ src/drivers/oreoled/oreoled.cpp | 81 ++++++++++++++++++++++++++++++++- 2 files changed, 90 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 00e36831841d..1cbb02552d4a 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -61,6 +61,12 @@ /** send bytes */ #define OREOLED_SEND_BYTES _OREOLEDIOC(3) +/** send reset */ +#define OREOLED_SEND_RESET _OREOLEDIOC(4) + +/** boot application */ +#define OREOLED_BL_BOOT_APP _OREOLEDIOC(5) + /* Oreo LED driver supports up to 4 leds */ #define OREOLED_NUM_LEDS 4 @@ -70,6 +76,9 @@ /* maximum command length that can be sent to LEDs */ #define OREOLED_CMD_LENGTH_MAX 24 +/* magic number used to verify the software reset is valid */ +#define OEROLED_RESET_NONCE 42 + /* enum passed to OREOLED_SET_MODE ioctl() * defined by hardware */ enum oreoled_pattern { @@ -97,6 +106,7 @@ enum oreoled_param { OREOLED_PARAM_REPEAT = 7, OREOLED_PARAM_PHASEOFFSET = 8, OREOLED_PARAM_MACRO = 9, + OREOLED_PARAM_RESET = 10, OREOLED_PARAM_ENUM_COUNT }; diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 3918a6575e85..e60e647c5b78 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -368,6 +368,45 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; + case OREOLED_SEND_RESET: + warnx("sending a reset..."); + /* send a reset */ + new_cmd.led_num = OREOLED_ALL_INSTANCES; + new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + new_cmd.buff[1] = OREOLED_PARAM_RESET; + new_cmd.buff[2] = OEROLED_RESET_NONCE; + new_cmd.num_bytes = 3; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + /* add command to queue for all healthy leds */ + if (_healthy[i]) { + warnx("sending a reset... to %i", i); + new_cmd.led_num = i; + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_BOOT_APP: + /* send a reset */ + //new_cmd.led_num = OREOLED_ALL_INSTANCES; + new_cmd.buff[0] = 0x01; + new_cmd.buff[1] = 0x80; + new_cmd.num_bytes = 2; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + /* add command to queue for all healthy leds */ + if (_healthy[i]) { + new_cmd.led_num = i; + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + return ret; + case OREOLED_SEND_BYTES: /* send bytes */ new_cmd = *((oreoled_cmd_t *) arg); @@ -447,7 +486,7 @@ OREOLED::send_cmd(oreoled_cmd_t new_cmd) void oreoled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50' 'macro 4' 'gencall' 'bytes 7 9 6'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'reset', 'rgb 30 40 50', 'macro 4', 'gencall', 'bytes 7 9 6'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", OREOLED_BASE_I2C_ADDR); @@ -648,6 +687,46 @@ oreoled_main(int argc, char *argv[]) exit(ret); } + /* send reset request to all LEDS */ + if (!strcmp(verb, "reset")) { + if (argc < 2) { + errx(1, "Usage: oreoled reset"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_SEND_RESET, 0)) != OK) { + errx(1, "failed to run macro"); + } + + close(fd); + exit(ret); + } + + /* send reset request to all LEDS */ + if (!strcmp(verb, "boot")) { + if (argc < 2) { + errx(1, "Usage: oreoled boot"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_BOOT_APP, 0)) != OK) { + errx(1, "failed to run macro"); + } + + close(fd); + exit(ret); + } + /* send general hardware call to all LEDS */ if (!strcmp(verb, "gencall")) { ret = g_oreoled->send_general_call(); From 1f7b17ed8b4d6b80d41e03dbdee73839f5de33d9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 31 Mar 2015 09:59:25 -0700 Subject: [PATCH 137/293] oreoled: added more perf counters and I2C reply bytes also fixed up getopt handling --- src/drivers/oreoled/oreoled.cpp | 75 ++++++++++++++++++++++++++++----- 1 file changed, 64 insertions(+), 11 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index e60e647c5b78..67b48e2a2134 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -69,7 +69,8 @@ #define OREOLED_NUM_LEDS 4 ///< maximum number of LEDs the oreo led driver can support #define OREOLED_BASE_I2C_ADDR 0x68 ///< base i2c address (7-bit) -#define OREOLED_TIMEOUT_MS 10000000U ///< timeout looking for battery 10seconds after startup +#define OPEOLED_I2C_RETRYCOUNT 2 ///< i2c retry count +#define OREOLED_TIMEOUT_USEC 10000000U ///< timeout looking for battery 10seconds after startup #define OREOLED_GENERALCALL_US 4000000U ///< general call sent every 4 seconds #define OREOLED_GENERALCALL_CMD 0x00 ///< general call command sent at regular intervals @@ -124,6 +125,13 @@ class OREOLED : public device::I2C ringbuffer::RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs uint64_t _last_gencall; uint64_t _start_time; ///< system time we first attempt to communicate with battery + + /* performance checking */ + perf_counter_t _call_perf; + perf_counter_t _gcall_perf; + perf_counter_t _probe_perf; + perf_counter_t _comms_errors; + perf_counter_t _reply_errors; }; /* for now, we only support one OREOLED */ @@ -142,7 +150,12 @@ OREOLED::OREOLED(int bus, int i2c_addr) : _work{}, _num_healthy(0), _cmd_queue(nullptr), - _last_gencall(0) + _last_gencall(0), + _call_perf(perf_alloc(PC_ELAPSED, "oreoled_call")), + _gcall_perf(perf_alloc(PC_ELAPSED, "oreoled_gcall")), + _probe_perf(perf_alloc(PC_ELAPSED, "oreoled_probe")), + _comms_errors(perf_alloc(PC_COUNT, "oreoled_comms_errors")), + _reply_errors(perf_alloc(PC_COUNT, "oreoled_reply_errors")) { /* initialise to unhealthy */ memset(_healthy, 0, sizeof(_healthy)); @@ -161,6 +174,13 @@ OREOLED::~OREOLED() if (_cmd_queue != nullptr) { delete _cmd_queue; } + + /* free perf counters */ + perf_free(_call_perf); + perf_free(_gcall_perf); + perf_free(_probe_perf); + perf_free(_comms_errors); + perf_free(_reply_errors); } int @@ -192,6 +212,9 @@ OREOLED::init() int OREOLED::probe() { + /* set retry count */ + _retries = OPEOLED_I2C_RETRYCOUNT; + /* always return true */ return OK; } @@ -202,13 +225,20 @@ OREOLED::info() /* print health info on each LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (!_healthy[i]) { - log("oreo %u: BAD", (int)i); + log("oreo %u: BAD", (unsigned)i); } else { - log("oreo %u: OK", (int)i); + log("oreo %u: OK", (unsigned)i); } } + /* display perf info */ + perf_print_counter(_call_perf); + perf_print_counter(_gcall_perf); + perf_print_counter(_probe_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_reply_errors); + return OK; } @@ -241,7 +271,7 @@ OREOLED::cycle() { /* check time since startup */ uint64_t now = hrt_absolute_time(); - bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_MS); + bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_USEC); /* if not leds found during start-up period, exit without rescheduling */ if (startup_timeout && _num_healthy == 0) { @@ -257,15 +287,23 @@ OREOLED::cycle() /* attempt to contact each unhealthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (!_healthy[i]) { + perf_begin(_probe_perf); + /* set I2C address */ set_address(OREOLED_BASE_I2C_ADDR + i); /* send I2C command and record health*/ - if (transfer(msg, sizeof(msg), nullptr, 0) == OK) { + uint8_t reply[2]; + if (transfer(msg, sizeof(msg), reply, 2) == OK) { _healthy[i] = true; _num_healthy++; - warnx("oreoled %d ok", (unsigned)i); + log("oreoled %u ok", (unsigned)i); + if (reply[0] != OREOLED_BASE_I2C_ADDR + i || + reply[1] != msg[0]) { + perf_count(_reply_errors); + } } + perf_end(_probe_perf); } } @@ -282,21 +320,36 @@ OREOLED::cycle() /* send valid messages to healthy LEDs */ if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { + /* start performance timer */ + perf_begin(_call_perf); + /* set I2C address */ set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); /* send I2C command */ - transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0); + + uint8_t reply[2]; + if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 2) != OK) { + perf_count(_comms_errors); + } else if (reply[0] != OREOLED_BASE_I2C_ADDR + next_cmd.led_num || + reply[1] != next_cmd.buff[0]) { + perf_count(_reply_errors); + } + + perf_end(_call_perf); } } /* send general call every 4 seconds*/ if ((now - _last_gencall) > OREOLED_GENERALCALL_US) { + perf_begin(_gcall_perf); send_general_call(); + perf_end(_gcall_perf); } /* schedule a fresh cycle call when the measurement is done */ work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); + } int @@ -749,18 +802,18 @@ oreoled_main(int argc, char *argv[]) } /* check led num */ - sendb.led_num = (uint8_t)strtol(argv[2], NULL, 0); + sendb.led_num = (uint8_t)strtol(argv[optind + 1], NULL, 0); if (sendb.led_num > 3) { errx(1, "led number must be between 0 ~ 3"); } /* get bytes */ - sendb.num_bytes = argc - 3; + sendb.num_bytes = argc - (optind+2); uint8_t byte_count; for (byte_count = 0; byte_count < sendb.num_bytes; byte_count++) { - sendb.buff[byte_count] = (uint8_t)strtol(argv[byte_count + 3], NULL, 0); + sendb.buff[byte_count] = (uint8_t)strtol(argv[byte_count + optind + 2], NULL, 0); } /* send bytes */ From 2c90b0628c47cc29385be11bb895a9c44f3c0eb6 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Tue, 31 Mar 2015 16:29:48 -0700 Subject: [PATCH 138/293] oreoled: added XOR crc check --- src/drivers/oreoled/oreoled.cpp | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 67b48e2a2134..6c53b070f9da 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -293,7 +293,7 @@ OREOLED::cycle() set_address(OREOLED_BASE_I2C_ADDR + i); /* send I2C command and record health*/ - uint8_t reply[2]; + uint8_t reply[2]; if (transfer(msg, sizeof(msg), reply, 2) == OK) { _healthy[i] = true; _num_healthy++; @@ -325,14 +325,24 @@ OREOLED::cycle() /* set I2C address */ set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); - /* send I2C command */ - uint8_t reply[2]; - if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 2) != OK) { + /* Calculate XOR CRC and append to the i2c write data */ + uint8_t i; + uint32_t next_cmd_xor = 0; + for(i = 0; i < next_cmd.num_bytes; i++) { + next_cmd_xor ^= next_cmd.buff[i]; + } + /*log("sending %d bytes a", next_cmd.num_bytes); + next_cmd.buff[++next_cmd.num_bytes] = next_cmd_xor; + log("sending %d bytes b", next_cmd.num_bytes);*/ + + /* send I2C command */ + uint8_t reply[3]; // First byte is ignored + if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 3) != OK) { perf_count(_comms_errors); - } else if (reply[0] != OREOLED_BASE_I2C_ADDR + next_cmd.led_num || - reply[1] != next_cmd.buff[0]) { - perf_count(_reply_errors); + } else if (reply[1] != OREOLED_BASE_I2C_ADDR + next_cmd.led_num || + reply[2] != next_cmd_xor) { + perf_count(_reply_errors); } perf_end(_call_perf); From 7cf0eae64e7a06d1bdec0945c37abd11b31e4dfd Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Mon, 6 Apr 2015 09:47:48 -0700 Subject: [PATCH 139/293] oreoled: add two way XOR checksum and retries --- src/drivers/drv_oreoled.h | 13 ++- src/drivers/oreoled/oreoled.cpp | 151 +++++++++----------------------- 2 files changed, 46 insertions(+), 118 deletions(-) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 1cbb02552d4a..5fe9d4ec317b 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -61,12 +61,6 @@ /** send bytes */ #define OREOLED_SEND_BYTES _OREOLEDIOC(3) -/** send reset */ -#define OREOLED_SEND_RESET _OREOLEDIOC(4) - -/** boot application */ -#define OREOLED_BL_BOOT_APP _OREOLEDIOC(5) - /* Oreo LED driver supports up to 4 leds */ #define OREOLED_NUM_LEDS 4 @@ -76,8 +70,11 @@ /* maximum command length that can be sent to LEDs */ #define OREOLED_CMD_LENGTH_MAX 24 -/* magic number used to verify the software reset is valid */ -#define OEROLED_RESET_NONCE 42 +/* maximum command length that can be read from LEDs */ +#define OREOLED_CMD_READ_LENGTH_MAX 10 + +/* maximum number of commands retries */ +#define OEROLED_COMMAND_RETRIES 10 /* enum passed to OREOLED_SET_MODE ioctl() * defined by hardware */ diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 6c53b070f9da..cbbd0c7bb8bc 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -70,7 +70,7 @@ #define OREOLED_NUM_LEDS 4 ///< maximum number of LEDs the oreo led driver can support #define OREOLED_BASE_I2C_ADDR 0x68 ///< base i2c address (7-bit) #define OPEOLED_I2C_RETRYCOUNT 2 ///< i2c retry count -#define OREOLED_TIMEOUT_USEC 10000000U ///< timeout looking for battery 10seconds after startup +#define OREOLED_TIMEOUT_USEC 10000000U ///< timeout looking for oreoleds 10seconds after startup #define OREOLED_GENERALCALL_US 4000000U ///< general call sent every 4 seconds #define OREOLED_GENERALCALL_CMD 0x00 ///< general call command sent at regular intervals @@ -273,16 +273,14 @@ OREOLED::cycle() uint64_t now = hrt_absolute_time(); bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_USEC); - /* if not leds found during start-up period, exit without rescheduling */ - if (startup_timeout && _num_healthy == 0) { - warnx("did not find oreoled"); - return; - } + /* prepare the response buffer */ + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; /* during startup period keep searching for unhealthy LEDs */ if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) { - /* prepare command to turn off LED*/ - uint8_t msg[] = {OREOLED_PATTERN_OFF}; + /* prepare command to turn off LED */ + /* add two bytes of pre-amble to for higher signal to noise ratio */ + uint8_t msg[] = {0xAA, 0x55, OREOLED_PATTERN_OFF, 0x00}; /* attempt to contact each unhealthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { @@ -292,16 +290,24 @@ OREOLED::cycle() /* set I2C address */ set_address(OREOLED_BASE_I2C_ADDR + i); - /* send I2C command and record health*/ - uint8_t reply[2]; - if (transfer(msg, sizeof(msg), reply, 2) == OK) { - _healthy[i] = true; - _num_healthy++; - log("oreoled %u ok", (unsigned)i); - if (reply[0] != OREOLED_BASE_I2C_ADDR + i || - reply[1] != msg[0]) { + /* Calculate XOR CRC and append to the i2c write data */ + msg[sizeof(msg)-1] = OREOLED_BASE_I2C_ADDR + i; + for(uint8_t j = 0; j < sizeof(msg)-1; j++) { + msg[sizeof(msg)-1] ^= msg[j]; + } + + /* send I2C command */ + if (transfer(msg, sizeof(msg), reply, 3) == OK) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + i && + reply[2] == msg[sizeof(msg)-1]) { + log("oreoled %u ok", (unsigned)i); + _healthy[i] = true; + _num_healthy++; + } else { perf_count(_reply_errors); } + } else { + perf_count(_comms_errors); } perf_end(_probe_perf); } @@ -327,22 +333,25 @@ OREOLED::cycle() set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); /* Calculate XOR CRC and append to the i2c write data */ - uint8_t i; - uint32_t next_cmd_xor = 0; - for(i = 0; i < next_cmd.num_bytes; i++) { + uint8_t next_cmd_xor = OREOLED_BASE_I2C_ADDR + next_cmd.led_num; + for(uint8_t i = 0; i < next_cmd.num_bytes; i++) { next_cmd_xor ^= next_cmd.buff[i]; } - /*log("sending %d bytes a", next_cmd.num_bytes); - next_cmd.buff[++next_cmd.num_bytes] = next_cmd_xor; - log("sending %d bytes b", next_cmd.num_bytes);*/ - - /* send I2C command */ - uint8_t reply[3]; // First byte is ignored - if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 3) != OK) { - perf_count(_comms_errors); - } else if (reply[1] != OREOLED_BASE_I2C_ADDR + next_cmd.led_num || - reply[2] != next_cmd_xor) { - perf_count(_reply_errors); + next_cmd.buff[next_cmd.num_bytes++] = next_cmd_xor; + + /* send I2C command with a retry limit */ + for(uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 3) == OK) { + if (reply[1] == (OREOLED_BASE_I2C_ADDR + next_cmd.led_num) && + reply[2] == next_cmd_xor) { + /* slave returned a valid response */ + break; + } else { + perf_count(_reply_errors); + } + } else { + perf_count(_comms_errors); + } } perf_end(_call_perf); @@ -367,6 +376,7 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = -ENODEV; oreoled_cmd_t new_cmd; + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; switch (cmd) { case OREOLED_SET_RGB: @@ -431,45 +441,6 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; - case OREOLED_SEND_RESET: - warnx("sending a reset..."); - /* send a reset */ - new_cmd.led_num = OREOLED_ALL_INSTANCES; - new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; - new_cmd.buff[1] = OREOLED_PARAM_RESET; - new_cmd.buff[2] = OEROLED_RESET_NONCE; - new_cmd.num_bytes = 3; - - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - /* add command to queue for all healthy leds */ - if (_healthy[i]) { - warnx("sending a reset... to %i", i); - new_cmd.led_num = i; - _cmd_queue->force(&new_cmd); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_BOOT_APP: - /* send a reset */ - //new_cmd.led_num = OREOLED_ALL_INSTANCES; - new_cmd.buff[0] = 0x01; - new_cmd.buff[1] = 0x80; - new_cmd.num_bytes = 2; - - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - /* add command to queue for all healthy leds */ - if (_healthy[i]) { - new_cmd.led_num = i; - _cmd_queue->force(&new_cmd); - ret = OK; - } - } - - return ret; - case OREOLED_SEND_BYTES: /* send bytes */ new_cmd = *((oreoled_cmd_t *) arg); @@ -549,7 +520,7 @@ OREOLED::send_cmd(oreoled_cmd_t new_cmd) void oreoled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'reset', 'rgb 30 40 50', 'macro 4', 'gencall', 'bytes 7 9 6'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50', 'macro 4', 'gencall', 'bytes 7 9 6'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", OREOLED_BASE_I2C_ADDR); @@ -750,46 +721,6 @@ oreoled_main(int argc, char *argv[]) exit(ret); } - /* send reset request to all LEDS */ - if (!strcmp(verb, "reset")) { - if (argc < 2) { - errx(1, "Usage: oreoled reset"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_SEND_RESET, 0)) != OK) { - errx(1, "failed to run macro"); - } - - close(fd); - exit(ret); - } - - /* send reset request to all LEDS */ - if (!strcmp(verb, "boot")) { - if (argc < 2) { - errx(1, "Usage: oreoled boot"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_BOOT_APP, 0)) != OK) { - errx(1, "failed to run macro"); - } - - close(fd); - exit(ret); - } - /* send general hardware call to all LEDS */ if (!strcmp(verb, "gencall")) { ret = g_oreoled->send_general_call(); From 79116c4bbd761f76302d5690ce7225d7faf9cc6b Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Thu, 9 Apr 2015 17:38:41 -0700 Subject: [PATCH 140/293] oreoled: bootloader support --- src/drivers/drv_oreoled.h | 61 +- src/drivers/oreoled/oreoled.cpp | 975 +++++++++++++++++++++++++++++++- 2 files changed, 1026 insertions(+), 10 deletions(-) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 5fe9d4ec317b..67ab198c0222 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -61,6 +61,30 @@ /** send bytes */ #define OREOLED_SEND_BYTES _OREOLEDIOC(3) +/** send reset */ +#define OREOLED_SEND_RESET _OREOLEDIOC(4) + +/** boot ping */ +#define OREOLED_BL_PING _OREOLEDIOC(5) + +/** boot version */ +#define OREOLED_BL_VER _OREOLEDIOC(6) + +/** boot write flash */ +#define OREOLED_BL_FLASH _OREOLEDIOC(7) + +/** boot application version */ +#define OREOLED_BL_APP_VER _OREOLEDIOC(8) + +/** boot application crc */ +#define OREOLED_BL_APP_CRC _OREOLEDIOC(9) + +/** boot startup colour */ +#define OREOLED_BL_SET_COLOUR _OREOLEDIOC(10) + +/** boot application */ +#define OREOLED_BL_BOOT_APP _OREOLEDIOC(11) + /* Oreo LED driver supports up to 4 leds */ #define OREOLED_NUM_LEDS 4 @@ -68,7 +92,7 @@ #define OREOLED_ALL_INSTANCES 0xff /* maximum command length that can be sent to LEDs */ -#define OREOLED_CMD_LENGTH_MAX 24 +#define OREOLED_CMD_LENGTH_MAX 70 /* maximum command length that can be read from LEDs */ #define OREOLED_CMD_READ_LENGTH_MAX 10 @@ -76,6 +100,39 @@ /* maximum number of commands retries */ #define OEROLED_COMMAND_RETRIES 10 +/* magic number used to verify the software reset is valid */ +#define OEROLED_RESET_NONCE 0x2A + +/* microseconds to hold-off between write and reads */ +#define OREOLED_WRITE_READ_HOLDOFF_US 500 + +/* microseconds to hold-off between retries */ +#define OREOLED_RETRY_HOLDOFF_US 200 + +#define OEROLED_BOOT_COMMAND_RETRIES 25 +#define OREOLED_BOOT_FLASH_WAITMS 10 + +#define OREOLED_BOOT_SUPPORTED_VER 0x01 + +#define OREOLED_BOOT_CMD_PING 0x40 +#define OREOLED_BOOT_CMD_BL_VER 0x41 +#define OREOLED_BOOT_CMD_APP_VER 0x42 +#define OREOLED_BOOT_CMD_APP_CRC 0x43 +#define OREOLED_BOOT_CMD_SET_COLOUR 0x44 + +#define OREOLED_BOOT_CMD_WRITE_FLASH_A 0x50 +#define OREOLED_BOOT_CMD_WRITE_FLASH_B 0x51 +#define OREOLED_BOOT_CMD_FINALISE_FLASH 0x55 + +#define OREOLED_BOOT_CMD_BOOT_APP 0x60 + +#define OREOLED_BOOT_CMD_PING_NONCE 0x2A +#define OREOLED_BOOT_CMD_BOOT_NONCE 0xA2 + +#define OREOLED_FW_FILE_SIZE_LIMIT 6144 +#define OREOLED_FW_VERSION 0x0100 +#define OREOLED_FW_FILE "/etc/firmware/oreoled.bin" + /* enum passed to OREOLED_SET_MODE ioctl() * defined by hardware */ enum oreoled_pattern { @@ -121,6 +178,8 @@ enum oreoled_macro { OREOLED_PARAM_MACRO_BLUE = 8, OREOLED_PARAM_MACRO_YELLOW = 9, OREOLED_PARAM_MACRO_WHITE = 10, + OREOLED_PARAM_MACRO_AUTOMOBILE = 11, + OREOLED_PARAM_MACRO_AVIATION = 12, OREOLED_PARAM_MACRO_ENUM_COUNT }; diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index cbbd0c7bb8bc..1437b3a688cb 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -70,7 +71,7 @@ #define OREOLED_NUM_LEDS 4 ///< maximum number of LEDs the oreo led driver can support #define OREOLED_BASE_I2C_ADDR 0x68 ///< base i2c address (7-bit) #define OPEOLED_I2C_RETRYCOUNT 2 ///< i2c retry count -#define OREOLED_TIMEOUT_USEC 10000000U ///< timeout looking for oreoleds 10seconds after startup +#define OREOLED_TIMEOUT_USEC 4000000U ///< timeout looking for oreoleds 4seconds after startup #define OREOLED_GENERALCALL_US 4000000U ///< general call sent every 4 seconds #define OREOLED_GENERALCALL_CMD 0x00 ///< general call command sent at regular intervals @@ -82,7 +83,7 @@ class OREOLED : public device::I2C { public: - OREOLED(int bus, int i2c_addr); + OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate); virtual ~OREOLED(); virtual int init(); @@ -118,13 +119,27 @@ class OREOLED : public device::I2C */ void cycle(); + int bootloader_app_reset(int led_num); + int bootloader_ping(int led_num); + uint8_t bootloader_version(int led_num); + uint16_t bootloader_app_version(int led_num); + uint16_t bootloader_app_checksum(int led_num); + int bootloader_set_colour(int led_num, uint8_t red, uint8_t green); + int bootloader_flash(int led_num); + int bootloader_boot(int led_num); + /* internal variables */ work_s _work; ///< work queue for scheduling reads bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED + bool _in_boot[OREOLED_NUM_LEDS]; ///< true for each LED that is in bootloader mode uint8_t _num_healthy; ///< number of healthy LEDs ringbuffer::RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs + uint8_t _num_inboot; ///< number of LEDs in bootloader uint64_t _last_gencall; uint64_t _start_time; ///< system time we first attempt to communicate with battery + bool _autoupdate; ///< true if the driver should update all LEDs + bool _alwaysupdate; ///< true if the driver should update all LEDs + bool _is_bootloading; ///< true if a bootloading operation is in progress /* performance checking */ perf_counter_t _call_perf; @@ -145,12 +160,16 @@ void oreoled_usage(); extern "C" __EXPORT int oreoled_main(int argc, char *argv[]); /* constructor */ -OREOLED::OREOLED(int bus, int i2c_addr) : +OREOLED::OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate) : I2C("oreoled", OREOLED0_DEVICE_PATH, bus, i2c_addr, 100000), _work{}, _num_healthy(0), + _num_inboot(0), _cmd_queue(nullptr), _last_gencall(0), + _autoupdate(autoupdate), + _alwaysupdate(alwaysupdate), + _is_bootloading(false), _call_perf(perf_alloc(PC_ELAPSED, "oreoled_call")), _gcall_perf(perf_alloc(PC_ELAPSED, "oreoled_gcall")), _probe_perf(perf_alloc(PC_ELAPSED, "oreoled_probe")), @@ -160,6 +179,9 @@ OREOLED::OREOLED(int bus, int i2c_addr) : /* initialise to unhealthy */ memset(_healthy, 0, sizeof(_healthy)); + /* initialise to in application */ + memset(_in_boot, 0, sizeof(_in_boot)); + /* capture startup time */ _start_time = hrt_absolute_time(); } @@ -303,7 +325,14 @@ OREOLED::cycle() log("oreoled %u ok", (unsigned)i); _healthy[i] = true; _num_healthy++; + + /* If slaves are in application record that so we can reset if we need to bootload */ + if(bootloader_ping(i) == OK) { + _in_boot[i] = true; + _num_inboot++; + } } else { + log("oreo reply errors: %u", (unsigned)_reply_errors); perf_count(_reply_errors); } } else { @@ -317,6 +346,78 @@ OREOLED::cycle() work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_STARTUP_INTERVAL_US)); return; + } else if(_alwaysupdate) { + /* attempt to update each healthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + _is_bootloading = true; + + /* reset the LED if it's not in the bootloader */ + /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ + if(!_in_boot[i]) + bootloader_app_reset(i); + + /* if the flashing was successful, boot the new app */ + if(bootloader_flash(i)) + bootloader_boot(i); + + _is_bootloading = false; + } + } + + /* mandatory updating has finished */ + _alwaysupdate = false; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, + USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); + return; + } else if(_autoupdate) { + /* attempt to update each healthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + _is_bootloading = true; + + /* reset the LED if it's not in the bootloader */ + /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ + if(!_in_boot[i]) + bootloader_app_reset(i); + + /* only flash LEDs with an old version of the applictioon */ + if(bootloader_app_version(i) != OREOLED_FW_VERSION) { + /* if the flashing was successful, boot the new app */ + if(bootloader_flash(i)) + bootloader_boot(i); + } else { + /* boot the application since we aren't flashing it */ + bootloader_boot(i); + } + + _is_bootloading = false; + } + } + + /* auto updating has finished */ + _autoupdate = false; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, + USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); + return; + } else if(_num_inboot > 0) { + /* boot any LEDs which are in bootloader mode */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_in_boot[i]) + bootloader_boot(i); + } + + /* ensure we don't get stuck in a loop */ + _num_inboot = 0; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, + USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); + return; } /* get next command from queue */ @@ -358,17 +459,582 @@ OREOLED::cycle() } } - /* send general call every 4 seconds*/ - if ((now - _last_gencall) > OREOLED_GENERALCALL_US) { + /* send general call every 4 seconds, if we aren't bootloading*/ + if (!_is_bootloading && ((now - _last_gencall) > OREOLED_GENERALCALL_US)) { perf_begin(_gcall_perf); send_general_call(); perf_end(_gcall_perf); } - /* schedule a fresh cycle call when the measurement is done */ + /* schedule a fresh cycle call when the command is sent */ work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); +} + +int +OREOLED::bootloader_app_reset(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + /* send a reset */ + boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + boot_cmd.buff[1] = OREOLED_PARAM_RESET; + boot_cmd.buff[2] = OEROLED_RESET_NONCE; + boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 4; + for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + } + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + /* send I2C command with a retry limit */ + for(uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { + if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && + reply[2] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + /* slave returned a valid response */ + ret = OK; + /* set this LED as being in boot mode now */ + _in_boot[led_num] = true; + _num_inboot++; + break; + } + } + } + + /* Allow time for the LED to reboot */ + usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + + _is_bootloading = false; + return ret; +} + +int +OREOLED::bootloader_ping(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_PING; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); + + /* Check the response */ + if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_PING && + reply[3] == OREOLED_BOOT_CMD_PING_NONCE && + reply[4] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + warnx("bl ping OK from LED %i", boot_cmd.led_num); + ret = OK; + break; + } else { + warnx("bl ping FAIL from LED %i", boot_cmd.led_num); + warnx("bl ping response ADDR: 0x%x", reply[1]); + warnx("bl ping response CMD: 0x%x", reply[2]); + warnx("bl ping response NONCE: 0x%x", reply[3]); + warnx("bl ping response XOR: 0x%x", reply[4]); + if(retry > 1) { + warnx("bl ping retrying LED %i", boot_cmd.led_num); + } else { + warnx("bl ping failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +uint8_t +OREOLED::bootloader_version(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint8_t ret = 0x00; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_BL_VER; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[k]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); + + /* Check the response */ + if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BL_VER && + reply[3] == OREOLED_BOOT_SUPPORTED_VER && + reply[4] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + warnx("bl ver from LED %i = %i", boot_cmd.led_num, reply[3]); + ret = reply[3]; + break; + } else { + warnx("bl ver response ADDR: 0x%x", reply[1]); + warnx("bl ver response CMD: 0x%x", reply[2]); + warnx("bl ver response VER: 0x%x", reply[3]); + warnx("bl ver response XOR: 0x%x", reply[4]); + if(retry > 1) { + warnx("bl ver retrying LED %i", boot_cmd.led_num); + } else { + warnx("bl ver failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +uint16_t +OREOLED::bootloader_app_version(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint16_t ret = 0x0000; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_VER; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + + /* Check the response */ + if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_APP_VER && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + warnx("bl app version OK from LED %i", boot_cmd.led_num); + warnx("bl app version msb: 0x%x", reply[3]); + warnx("bl app version lsb: 0x%x", reply[4]); + ret = ((reply[3] << 8) | reply[4]); + break; + } else { + warnx("bl app version FAIL from LED %i", boot_cmd.led_num); + warnx("bl app version response ADDR: 0x%x", reply[1]); + warnx("bl app version response CMD: 0x%x", reply[2]); + warnx("bl app version response VER H: 0x%x", reply[3]); + warnx("bl app version response VER L: 0x%x", reply[4]); + warnx("bl app version response XOR: 0x%x", reply[5]); + if(retry > 1) { + warnx("bl app version retrying LED %i", boot_cmd.led_num); + } else { + warnx("bl app version failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +uint16_t +OREOLED::bootloader_app_checksum(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint16_t ret = 0x0000; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_CRC; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + + /* Check the response */ + if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_APP_CRC && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + warnx("bl app checksum OK from LED %i", boot_cmd.led_num); + warnx("bl app checksum msb: 0x%x", reply[3]); + warnx("bl app checksum lsb: 0x%x", reply[4]); + ret = ((reply[3] << 8) | reply[4]); + break; + } else { + warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); + warnx("bl app checksum response ADDR: 0x%x", reply[1]); + warnx("bl app checksum response CMD: 0x%x", reply[2]); + warnx("bl app checksum response VER H: 0x%x", reply[3]); + warnx("bl app checksum response VER L: 0x%x", reply[4]); + warnx("bl app checksum response XOR: 0x%x", reply[5]); + if(retry > 1) { + warnx("bl app checksum retrying LED %i", boot_cmd.led_num); + } else { + warnx("bl app checksum failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +int +OREOLED::bootloader_set_colour(int led_num, uint8_t red, uint8_t green) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_SET_COLOUR; + boot_cmd.buff[1] = red; + boot_cmd.buff[2] = green; + boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 4; + for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_SET_COLOUR && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + warnx("bl set colour OK from LED %i", boot_cmd.led_num); + ret = OK; + break; + } else { + warnx("bl set colour FAIL from LED %i", boot_cmd.led_num); + warnx("bl set colour response ADDR: 0x%x", reply[1]); + warnx("bl set colour response CMD: 0x%x", reply[2]); + warnx("bl set colour response XOR: 0x%x", reply[3]); + if(retry > 1) { + warnx("bl app colour retrying LED %i", boot_cmd.led_num); + } else { + warnx("bl app colour failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +int +OREOLED::bootloader_flash(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + /* Open the bootloader file */ + int fd = ::open(OREOLED_FW_FILE, O_RDONLY); + + /* check for error opening the file */ + if (fd < 0) + return -1; + + struct stat s; + + /* attempt to stat the file */ + if (stat(OREOLED_FW_FILE, &s) != 0) + return -1; + + /* sanity-check file size */ + if (s.st_size > OREOLED_FW_FILE_SIZE_LIMIT) + return -1; + + uint8_t *buf = new uint8_t[s.st_size]; + + /* check that the buffer has been allocated */ + if (buf == NULL) + return -1; + + /* check that the firmware can be read into the buffer */ + if (::read(fd, buf, s.st_size) != s.st_size) + return -1; + + ::close(fd); + + /* calculate flash pages (rounded up to nearest integer) */ + uint8_t flash_pages = ((s.st_size + 64 - 1) / 64); + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + /* Loop through flash pages */ + for(uint8_t page_idx = 0; page_idx < flash_pages; page_idx++) { + + /* Send the first half of the 64 byte flash page */ + memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); + boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_A; + boot_cmd.buff[1] = page_idx; + memcpy(boot_cmd.buff+2, buf+(page_idx*64), 32); + boot_cmd.buff[32+2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 32+3; + for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[k]; + } + + for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_A && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + warnx("bl flash %ia OK for LED %i", page_idx, boot_cmd.led_num); + break; + } else { + warnx("bl flash %ia FAIL for LED %i", page_idx, boot_cmd.led_num); + warnx("bl flash %ia response ADDR: 0x%x", page_idx, reply[1]); + warnx("bl flash %ia response CMD: 0x%x", page_idx, reply[2]); + warnx("bl flash %ia response XOR: 0x%x", page_idx, reply[3]); + if(retry > 1) { + warnx("bl flash %ia retrying LED %i", page_idx, boot_cmd.led_num); + } else { + warnx("bl flash %ia failed on LED %i", page_idx, boot_cmd.led_num); + return -1; + } + } + } + + /* Send the second half of the 64 byte flash page */ + memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); + boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_B; + memcpy(boot_cmd.buff+1, buf+(page_idx*64)+32, 32); + boot_cmd.buff[32+1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 32+2; + for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[k]; + } + + for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_B && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + warnx("bl flash %ib OK for LED %i", page_idx, boot_cmd.led_num); + break; + } else { + warnx("bl flash %ib FAIL for LED %i", page_idx, boot_cmd.led_num); + warnx("bl flash %ib response ADDR: 0x%x", page_idx, reply[1]); + warnx("bl flash %ib response CMD: 0x%x", page_idx, reply[2]); + warnx("bl flash %ib response XOR: 0x%x", page_idx, reply[3]); + if(retry > 1) { + warnx("bl flash %ib retrying LED %i", page_idx, boot_cmd.led_num); + } else { + errx(1, "bl flash %ib failed on LED %i", page_idx, boot_cmd.led_num); + return -1; + } + } + } + + /* Sleep to allow flash to write */ + /* Wait extra long on the first write, to allow time for EEPROM updates */ + if(page_idx == 0) { + usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + } else { + usleep(OREOLED_BOOT_FLASH_WAITMS*1000); + } + } + + /* Calculate a 16 bit XOR checksum of the flash */ + /* Skip first two bytes which are modified by the bootloader */ + uint16_t app_checksum = 0x0000; + for(uint16_t j = 2; j < s.st_size; j+=2) { + app_checksum ^= (buf[j] << 8) | buf[j+1]; + } + warnx("bl finalise length = %i", s.st_size); + warnx("bl finalise checksum = %i", app_checksum); + + /* Flash writes must have succeeded so finalise the flash */ + boot_cmd.buff[0] = OREOLED_BOOT_CMD_FINALISE_FLASH; + boot_cmd.buff[1] = (uint8_t)(OREOLED_FW_VERSION >> 8); + boot_cmd.buff[2] = (uint8_t)(OREOLED_FW_VERSION & 0xFF); + boot_cmd.buff[3] = (uint8_t)(s.st_size >> 8); + boot_cmd.buff[4] = (uint8_t)(s.st_size & 0xFF); + boot_cmd.buff[5] = (uint8_t)(app_checksum >> 8); + boot_cmd.buff[6] = (uint8_t)(app_checksum & 0xFF); + boot_cmd.buff[7] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 8; + for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[k]; + } + + /* Try to finalise for twice the amount of normal retries */ + for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES*2; retry > 0; retry--) { + /* Send the I2C Write */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_FINALISE_FLASH && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + warnx("bl finalise OK from LED %i", boot_cmd.led_num); + break; + } else { + warnx("bl finalise response ADDR: 0x%x", reply[1]); + warnx("bl finalise response CMD: 0x%x", reply[2]); + warnx("bl finalise response XOR: 0x%x", reply[3]); + if(retry > 1) { + warnx("bl finalise retrying LED %i", boot_cmd.led_num); + } else { + warnx("bl finalise failed on LED %i", boot_cmd.led_num); + return -1; + } + } + } + + /* allow time for flash to finalise */ + usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + + /* clean up file buffer */ + delete buf; + + _is_bootloading = false; + return 1; +} + +int +OREOLED::bootloader_boot(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_BOOT_APP; + boot_cmd.buff[1] = OREOLED_BOOT_CMD_BOOT_NONCE; + boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 3; + for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[k]; + } + + for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write */ + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BOOT_APP && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + warnx("bl boot OK from LED %i", boot_cmd.led_num); + /* decrement the inboot counter so we don't get confused */ + _in_boot[led_num] = false; + _num_inboot--; + ret = OK; + break; + } else if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BOOT_NONCE && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + warnx("bl boot error from LED %i: no app", boot_cmd.led_num); + break; + } else { + warnx("bl boot response ADDR: 0x%x", reply[1]); + warnx("bl boot response CMD: 0x%x", reply[2]); + warnx("bl boot response XOR: 0x%x", reply[3]); + if(retry > 1) { + warnx("bl boot retrying LED %i", boot_cmd.led_num); + } else { + warnx("bl boot failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + /* allow time for the LEDs to boot */ + usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + + _is_bootloading = false; + return ret; } int @@ -376,7 +1042,6 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = -ENODEV; oreoled_cmd_t new_cmd; - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; switch (cmd) { case OREOLED_SET_RGB: @@ -441,6 +1106,119 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; + case OREOLED_SEND_RESET: + /* send a reset */ + new_cmd.led_num = OREOLED_ALL_INSTANCES; + new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + new_cmd.buff[1] = OREOLED_PARAM_RESET; + new_cmd.buff[2] = OEROLED_RESET_NONCE; + new_cmd.num_bytes = 3; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + /* add command to queue for all healthy leds */ + if (_healthy[i]) { + warnx("sending a reset... to %i", i); + new_cmd.led_num = i; + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_PING: + _is_bootloading = true; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_ping(i); + ret = OK; + } + } + + _is_bootloading = false; + return ret; + + case OREOLED_BL_VER: + _is_bootloading = true; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_version(i); + ret = OK; + } + } + + _is_bootloading = false; + return ret; + + case OREOLED_BL_FLASH: + _is_bootloading = true; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_flash(i); + ret = OK; + } + } + + _is_bootloading = false; + return ret; + + case OREOLED_BL_APP_VER: + _is_bootloading = true; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_app_version(i); + ret = OK; + } + } + + _is_bootloading = false; + return ret; + + case OREOLED_BL_APP_CRC: + _is_bootloading = true; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_app_checksum(i); + ret = OK; + } + } + + _is_bootloading = false; + return ret; + + case OREOLED_BL_SET_COLOUR: + _is_bootloading = true; + new_cmd.led_num = OREOLED_ALL_INSTANCES; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_set_colour(i, ((oreoled_rgbset_t *) arg)->red, ((oreoled_rgbset_t *) arg)->green); + ret = OK; + } + } + + _is_bootloading = false; + return ret; + + case OREOLED_BL_BOOT_APP: + _is_bootloading = true; + new_cmd.led_num = OREOLED_ALL_INSTANCES; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + bootloader_boot(i); + ret = OK; + } + } + + _is_bootloading = false; + return ret; + case OREOLED_SEND_BYTES: /* send bytes */ new_cmd = *((oreoled_cmd_t *) arg); @@ -520,7 +1298,8 @@ OREOLED::send_cmd(oreoled_cmd_t new_cmd) void oreoled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50', 'macro 4', 'gencall', 'bytes 7 9 6'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'reset', 'rgb 30 40 50', 'macro 4', 'gencall', 'bytes 7 9 6'"); + warnx("bootloader commands: try 'blping', 'blver', 'blappver', 'blappcrc', 'blcolour ', 'blflash', 'blboot'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", OREOLED_BASE_I2C_ADDR); @@ -571,8 +1350,22 @@ oreoled_main(int argc, char *argv[]) i2cdevice = PX4_I2C_BUS_LED; } + /* handle autoupdate flag */ + bool autoupdate = false; + if (argc > 2 && !strcmp(argv[2], "autoupdate")) { + warnx("autoupdate enabled"); + autoupdate = true; + } + + /* handle autoupdate flag */ + bool alwaysupdate = false; + if (argc > 2 && !strcmp(argv[2], "alwaysupdate")) { + warnx("alwaysupdate enabled"); + alwaysupdate = true; + } + /* instantiate driver */ - g_oreoled = new OREOLED(i2cdevice, i2c_addr); + g_oreoled = new OREOLED(i2cdevice, i2c_addr, autoupdate, alwaysupdate); /* check if object was created */ if (g_oreoled == nullptr) { @@ -721,6 +1514,170 @@ oreoled_main(int argc, char *argv[]) exit(ret); } + /* send reset request to all LEDS */ + if (!strcmp(verb, "reset")) { + if (argc < 2) { + errx(1, "Usage: oreoled reset"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_SEND_RESET, 0)) != OK) { + errx(1, "failed to run macro"); + } + + close(fd); + exit(ret); + } + + /* attempt to flash all LEDS in bootloader mode*/ + if (!strcmp(verb, "blflash")) { + if (argc < 2) { + errx(1, "Usage: oreoled blflash"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_FLASH, 0)) != OK) { + errx(1, "failed to run flash"); + } + + close(fd); + exit(ret); + } + + /* send bootloader boot request to all LEDS */ + if (!strcmp(verb, "blboot")) { + if (argc < 2) { + errx(1, "Usage: oreoled blboot"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_BOOT_APP, 0)) != OK) { + errx(1, "failed to run boot"); + } + + close(fd); + exit(ret); + } + + /* send bootloader ping all LEDs */ + if (!strcmp(verb, "blping")) { + if (argc < 2) { + errx(1, "Usage: oreoled blping"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_PING, 0)) != OK) { + errx(1, "failed to run blping"); + } + + close(fd); + exit(ret); + } + + /* ask all LEDs for their bootloader version */ + if (!strcmp(verb, "blver")) { + if (argc < 2) { + errx(1, "Usage: oreoled blver"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_VER, 0)) != OK) { + errx(1, "failed to get bootloader version"); + } + + close(fd); + exit(ret); + } + + /* ask all LEDs for their application version */ + if (!strcmp(verb, "blappver")) { + if (argc < 2) { + errx(1, "Usage: oreoled blappver"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_APP_VER, 0)) != OK) { + errx(1, "failed to get boot app version"); + } + + close(fd); + exit(ret); + } + + /* ask all LEDs for their application crc */ + if (!strcmp(verb, "blappcrc")) { + if (argc < 2) { + errx(1, "Usage: oreoled blappcrc"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + if ((ret = ioctl(fd, OREOLED_BL_APP_CRC, 0)) != OK) { + errx(1, "failed to get boot app crc"); + } + + close(fd); + exit(ret); + } + + /* set the default bootloader LED colour on all LEDs */ + if (!strcmp(verb, "blcolour")) { + if (argc < 4) { + errx(1, "Usage: oreoled blcolour "); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + uint8_t red = (uint8_t)strtol(argv[2], NULL, 0); + uint8_t green = (uint8_t)strtol(argv[3], NULL, 0); + oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, 0}; + + if ((ret = ioctl(fd, OREOLED_BL_SET_COLOUR, (unsigned long)&rgb_set)) != OK) { + errx(1, "failed to set boot startup colours"); + } + + close(fd); + exit(ret); + } + /* send general hardware call to all LEDS */ if (!strcmp(verb, "gencall")) { ret = g_oreoled->send_general_call(); From ede5c4f7fb4c607a4b0f8ec27156b18a7c9be287 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Thu, 9 Apr 2015 20:38:52 -0700 Subject: [PATCH 141/293] oreoled: smarter handling of ready state * Hardcode 15s sleep no longer required after calling `oreoled start` * `oreoled start` waits until initialisation/booting is finished for a maximum of 20 seconds * Changed order of reset/flash/boot for a better UX --- src/drivers/oreoled/oreoled.cpp | 79 ++++++++++++++++++++++++++++----- 1 file changed, 69 insertions(+), 10 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 1437b3a688cb..f194bdbb828b 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -71,7 +71,7 @@ #define OREOLED_NUM_LEDS 4 ///< maximum number of LEDs the oreo led driver can support #define OREOLED_BASE_I2C_ADDR 0x68 ///< base i2c address (7-bit) #define OPEOLED_I2C_RETRYCOUNT 2 ///< i2c retry count -#define OREOLED_TIMEOUT_USEC 4000000U ///< timeout looking for oreoleds 4seconds after startup +#define OREOLED_TIMEOUT_USEC 2000000U ///< timeout looking for oreoleds 2 seconds after startup #define OREOLED_GENERALCALL_US 4000000U ///< general call sent every 4 seconds #define OREOLED_GENERALCALL_CMD 0x00 ///< general call command sent at regular intervals @@ -97,6 +97,9 @@ class OREOLED : public device::I2C /* send cmd to an LEDs (used for testing only) */ int send_cmd(oreoled_cmd_t sb); + /* returns true once the driver finished bootloading and ready for commands */ + bool is_ready(); + private: /** @@ -140,6 +143,7 @@ class OREOLED : public device::I2C bool _autoupdate; ///< true if the driver should update all LEDs bool _alwaysupdate; ///< true if the driver should update all LEDs bool _is_bootloading; ///< true if a bootloading operation is in progress + bool _is_ready; ///< set to true once the driver has completly initialised /* performance checking */ perf_counter_t _call_perf; @@ -170,6 +174,7 @@ OREOLED::OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate) : _autoupdate(autoupdate), _alwaysupdate(alwaysupdate), _is_bootloading(false), + _is_ready(false), _call_perf(perf_alloc(PC_ELAPSED, "oreoled_call")), _gcall_perf(perf_alloc(PC_ELAPSED, "oreoled_gcall")), _probe_perf(perf_alloc(PC_ELAPSED, "oreoled_probe")), @@ -347,7 +352,7 @@ OREOLED::cycle() USEC2TICK(OREOLED_STARTUP_INTERVAL_US)); return; } else if(_alwaysupdate) { - /* attempt to update each healthy LED */ + /* reset each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { _is_bootloading = true; @@ -356,10 +361,30 @@ OREOLED::cycle() /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ if(!_in_boot[i]) bootloader_app_reset(i); + + _is_bootloading = false; + } + } + + /* attempt to update each healthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + _is_bootloading = true; /* if the flashing was successful, boot the new app */ - if(bootloader_flash(i)) - bootloader_boot(i); + bootloader_flash(i); + + _is_bootloading = false; + } + } + + /* boot each healthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + _is_bootloading = true; + + /* boot the application */ + bootloader_boot(i); _is_bootloading = false; } @@ -373,7 +398,7 @@ OREOLED::cycle() USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); return; } else if(_autoupdate) { - /* attempt to update each healthy LED */ + /* reset each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { _is_bootloading = true; @@ -382,21 +407,38 @@ OREOLED::cycle() /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ if(!_in_boot[i]) bootloader_app_reset(i); + + _is_bootloading = false; + } + } + + /* attempt to update each healthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + _is_bootloading = true; /* only flash LEDs with an old version of the applictioon */ if(bootloader_app_version(i) != OREOLED_FW_VERSION) { /* if the flashing was successful, boot the new app */ - if(bootloader_flash(i)) - bootloader_boot(i); - } else { - /* boot the application since we aren't flashing it */ - bootloader_boot(i); + bootloader_flash(i); } _is_bootloading = false; } } + /* boot each healthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + _is_bootloading = true; + + /* boot the application */ + bootloader_boot(i); + + _is_bootloading = false; + } + } + /* auto updating has finished */ _autoupdate = false; @@ -418,6 +460,9 @@ OREOLED::cycle() work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); return; + } else if(!_is_ready) { + /* indicate a ready state since startup has finished */ + _is_ready = true; } /* get next command from queue */ @@ -1295,6 +1340,13 @@ OREOLED::send_cmd(oreoled_cmd_t new_cmd) return ret; } +/* return the internal _is_ready flag indicating if initialisation is complete */ +bool +OREOLED::is_ready() +{ + return _is_ready; +} + void oreoled_usage() { @@ -1379,6 +1431,13 @@ oreoled_main(int argc, char *argv[]) errx(1, "failed to start driver"); } + /* wait for up to 20 seconds for the driver become ready */ + for(uint8_t i = 0; i < 20; i++) { + if(g_oreoled->is_ready()) + break; + sleep(1); + } + exit(0); } From a062737ad41401dfc2f781bc24cbc4f182468eb9 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Mon, 4 May 2015 15:49:52 +0800 Subject: [PATCH 142/293] oreoled: bump firmware version This is bad and needs to be handled better in the future... --- src/drivers/drv_oreoled.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 67ab198c0222..e40ab8f09802 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -130,7 +130,7 @@ #define OREOLED_BOOT_CMD_BOOT_NONCE 0xA2 #define OREOLED_FW_FILE_SIZE_LIMIT 6144 -#define OREOLED_FW_VERSION 0x0100 +#define OREOLED_FW_VERSION 0x0101 #define OREOLED_FW_FILE "/etc/firmware/oreoled.bin" /* enum passed to OREOLED_SET_MODE ioctl() From b34eb1a30b2a999970b47f93aeb2042c9adcdc94 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Tue, 5 May 2015 14:13:18 +0800 Subject: [PATCH 143/293] oreoled: update based on fw checksum not version Also remove redundant safety locking of the driver while bootloading. --- src/drivers/drv_oreoled.h | 1 - src/drivers/oreoled/oreoled.cpp | 83 ++++++++++++++++++++++----------- 2 files changed, 56 insertions(+), 28 deletions(-) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index e40ab8f09802..b44e223e6dee 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -130,7 +130,6 @@ #define OREOLED_BOOT_CMD_BOOT_NONCE 0xA2 #define OREOLED_FW_FILE_SIZE_LIMIT 6144 -#define OREOLED_FW_VERSION 0x0101 #define OREOLED_FW_FILE "/etc/firmware/oreoled.bin" /* enum passed to OREOLED_SET_MODE ioctl() diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index f194bdbb828b..cdfd58c0b7c9 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -130,6 +130,7 @@ class OREOLED : public device::I2C int bootloader_set_colour(int led_num, uint8_t red, uint8_t green); int bootloader_flash(int led_num); int bootloader_boot(int led_num); + uint16_t bootloader_fw_checksum(void); /* internal variables */ work_s _work; ///< work queue for scheduling reads @@ -144,6 +145,7 @@ class OREOLED : public device::I2C bool _alwaysupdate; ///< true if the driver should update all LEDs bool _is_bootloading; ///< true if a bootloading operation is in progress bool _is_ready; ///< set to true once the driver has completly initialised + uint16_t _fw_checksum; ///< the current 16bit XOR checksum of the built in oreoled firmware binary /* performance checking */ perf_counter_t _call_perf; @@ -175,6 +177,7 @@ OREOLED::OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate) : _alwaysupdate(alwaysupdate), _is_bootloading(false), _is_ready(false), + _fw_checksum(0x0000), _call_perf(perf_alloc(PC_ELAPSED, "oreoled_call")), _gcall_perf(perf_alloc(PC_ELAPSED, "oreoled_gcall")), _probe_perf(perf_alloc(PC_ELAPSED, "oreoled_probe")), @@ -355,38 +358,26 @@ OREOLED::cycle() /* reset each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { - _is_bootloading = true; - /* reset the LED if it's not in the bootloader */ /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ if(!_in_boot[i]) bootloader_app_reset(i); - - _is_bootloading = false; } } /* attempt to update each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { - _is_bootloading = true; - - /* if the flashing was successful, boot the new app */ + /* flash the new firmware */ bootloader_flash(i); - - _is_bootloading = false; } } /* boot each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { - _is_bootloading = true; - /* boot the application */ bootloader_boot(i); - - _is_bootloading = false; } } @@ -401,41 +392,29 @@ OREOLED::cycle() /* reset each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { - _is_bootloading = true; - /* reset the LED if it's not in the bootloader */ /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ if(!_in_boot[i]) bootloader_app_reset(i); - - _is_bootloading = false; } } /* attempt to update each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { - _is_bootloading = true; - /* only flash LEDs with an old version of the applictioon */ - if(bootloader_app_version(i) != OREOLED_FW_VERSION) { - /* if the flashing was successful, boot the new app */ + if(bootloader_app_checksum(i) != bootloader_fw_checksum()) { + /* flash the new firmware */ bootloader_flash(i); } - - _is_bootloading = false; } } /* boot each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { - _is_bootloading = true; - /* boot the application */ bootloader_boot(i); - - _is_bootloading = false; } } @@ -1082,6 +1061,56 @@ OREOLED::bootloader_boot(int led_num) return ret; } +uint16_t +OREOLED::bootloader_fw_checksum(void) +{ + /* Calculate the 16 bit XOR checksum of the firmware on the first call of this function */ + if(_fw_checksum == 0x0000) { + /* Open the bootloader file */ + int fd = ::open(OREOLED_FW_FILE, O_RDONLY); + + /* check for error opening the file */ + if (fd < 0) + return -1; + + struct stat s; + + /* attempt to stat the file */ + if (stat(OREOLED_FW_FILE, &s) != 0) + return -1; + + /* sanity-check file size */ + if (s.st_size > OREOLED_FW_FILE_SIZE_LIMIT) + return -1; + + uint8_t *buf = new uint8_t[s.st_size]; + + /* check that the buffer has been allocated */ + if (buf == NULL) + return -1; + + /* check that the firmware can be read into the buffer */ + if (::read(fd, buf, s.st_size) != s.st_size) + return -1; + + ::close(fd); + + /* Calculate a 16 bit XOR checksum of the flash */ + /* Skip first two bytes which are modified by the bootloader */ + uint16_t app_checksum = 0x0000; + for(uint16_t j = 2; j < s.st_size; j+=2) { + app_checksum ^= (buf[j] << 8) | buf[j+1]; + } + warnx("bl finalise length = %i", s.st_size); + warnx("bl finalise checksum = %i", app_checksum); + + /* Store the checksum so it's only calculated once */ + _fw_checksum = app_checksum; + } + + return _fw_checksum; +} + int OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) { From 86de52b14c1a2723edec706cdffde5494b93ca7b Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Thu, 7 May 2015 16:46:20 +0800 Subject: [PATCH 144/293] oreoled: coerce unhealthy LEDs --- src/drivers/drv_oreoled.h | 1 + src/drivers/oreoled/oreoled.cpp | 193 ++++++++++++++++++++++++++++---- 2 files changed, 173 insertions(+), 21 deletions(-) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index b44e223e6dee..1f7911157006 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -160,6 +160,7 @@ enum oreoled_param { OREOLED_PARAM_PHASEOFFSET = 8, OREOLED_PARAM_MACRO = 9, OREOLED_PARAM_RESET = 10, + OREOLED_PARAM_APP_CHECKSUM = 11, OREOLED_PARAM_ENUM_COUNT }; diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index cdfd58c0b7c9..1a152f046eb9 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -123,6 +123,8 @@ class OREOLED : public device::I2C void cycle(); int bootloader_app_reset(int led_num); + int bootloader_app_ping(int led_num); + uint16_t bootloader_inapp_checksum(int led_num); int bootloader_ping(int led_num); uint8_t bootloader_version(int led_num); uint16_t bootloader_app_version(int led_num); @@ -131,6 +133,7 @@ class OREOLED : public device::I2C int bootloader_flash(int led_num); int bootloader_boot(int led_num); uint16_t bootloader_fw_checksum(void); + int bootloader_coerce_healthy(void); /* internal variables */ work_s _work; ///< work queue for scheduling reads @@ -381,6 +384,20 @@ OREOLED::cycle() } } + /* double check each unhealthy LED */ + /* this re-checks "unhealthy" LEDs as they can sometimes power up with the wrong ID, */ + /* but will have the correct ID once they jump to the application and be healthy again */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (!_healthy[i] && bootloader_app_ping(i) == OK) { + /* mark as healthy */ + _healthy[i] = true; + _num_healthy++; + } + } + + /* coerce LEDs with startup issues to be healthy again */ + bootloader_coerce_healthy(); + /* mandatory updating has finished */ _alwaysupdate = false; @@ -389,33 +406,50 @@ OREOLED::cycle() USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); return; } else if(_autoupdate) { - /* reset each healthy LED */ + /* check booted oreoleds to see if the app can report it's checksum (release versions >= v1.2) */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - /* reset the LED if it's not in the bootloader */ - /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ - if(!_in_boot[i]) + if (_healthy[i] && !_in_boot[i]) { + /* put any out of date oreoleds into bootloader mode */ + /* being in bootloader mode signals to be code below that the will likey need updating */ + if(bootloader_inapp_checksum(i) != bootloader_fw_checksum()) { bootloader_app_reset(i); + } } } - /* attempt to update each healthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - /* only flash LEDs with an old version of the applictioon */ - if(bootloader_app_checksum(i) != bootloader_fw_checksum()) { - /* flash the new firmware */ - bootloader_flash(i); + /* reset all healthy oreoleds if the number of outdated oreoled's is > 0 */ + /* this is done for consistency, so if only one oreoled is updating, all LEDs show the same behaviour */ + /* otherwise a single oreoled could appear broken or failed. */ + if(_num_inboot > 0) { + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && !_in_boot[i]) { + /* reset the LED if it's not in the bootloader */ + /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ + bootloader_app_reset(i); } } - } - /* boot each healthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - /* boot the application */ - bootloader_boot(i); + /* update each outdated and healthy LED in bootloader mode */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && _in_boot[i]) { + /* only flash LEDs with an old version of the applictioon */ + if(bootloader_app_checksum(i) != bootloader_fw_checksum()) { + /* flash the new firmware */ + bootloader_flash(i); + } + } } + + /* boot each healthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && _in_boot[i]) { + /* boot the application */ + bootloader_boot(i); + } + } + + /* coerce LEDs with startup issues to be healthy again */ + bootloader_coerce_healthy(); } /* auto updating has finished */ @@ -426,12 +460,15 @@ OREOLED::cycle() USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); return; } else if(_num_inboot > 0) { - /* boot any LEDs which are in bootloader mode */ + /* boot any LEDs which are in still in bootloader mode */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_in_boot[i]) bootloader_boot(i); } + /* coerce LEDs with startup issues to be healthy again */ + bootloader_coerce_healthy(); + /* ensure we don't get stuck in a loop */ _num_inboot = 0; @@ -544,6 +581,100 @@ OREOLED::bootloader_app_reset(int led_num) return ret; } +int +OREOLED::bootloader_app_ping(int led_num) +{ + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + /* send a pattern off command */ + boot_cmd.buff[0] = 0xAA; + boot_cmd.buff[1] = 0x55; + boot_cmd.buff[2] = OREOLED_PATTERN_OFF; + boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 4; + for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + /* send I2C command with a retry limit */ + for(uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { + if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && + reply[2] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + /* slave returned a valid response */ + ret = OK; + break; + } + } + } + + return ret; +} + +uint16_t +OREOLED::bootloader_inapp_checksum(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint16_t ret = 0x0000; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + boot_cmd.buff[1] = OREOLED_PARAM_APP_CHECKSUM; + boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 3; + for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { + boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + } + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for(uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + + /* Check the response */ + if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_PARAM_APP_CHECKSUM && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + warnx("bl app checksum OK from LED %i", boot_cmd.led_num); + warnx("bl app checksum msb: 0x%x", reply[3]); + warnx("bl app checksum lsb: 0x%x", reply[4]); + ret = ((reply[3] << 8) | reply[4]); + break; + } else { + warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); + warnx("bl app checksum response ADDR: 0x%x", reply[1]); + warnx("bl app checksum response CMD: 0x%x", reply[2]); + warnx("bl app checksum response VER H: 0x%x", reply[3]); + warnx("bl app checksum response VER L: 0x%x", reply[4]); + warnx("bl app checksum response XOR: 0x%x", reply[5]); + if(retry > 1) { + warnx("bl app checksum retrying LED %i", boot_cmd.led_num); + } else { + warnx("bl app checksum failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + int OREOLED::bootloader_ping(int led_num) { @@ -950,8 +1081,8 @@ OREOLED::bootloader_flash(int led_num) /* Flash writes must have succeeded so finalise the flash */ boot_cmd.buff[0] = OREOLED_BOOT_CMD_FINALISE_FLASH; - boot_cmd.buff[1] = (uint8_t)(OREOLED_FW_VERSION >> 8); - boot_cmd.buff[2] = (uint8_t)(OREOLED_FW_VERSION & 0xFF); + boot_cmd.buff[1] = *buf; /* First two bytes of the file buffer are the version number */ + boot_cmd.buff[2] = *buf+1; boot_cmd.buff[3] = (uint8_t)(s.st_size >> 8); boot_cmd.buff[4] = (uint8_t)(s.st_size & 0xFF); boot_cmd.buff[5] = (uint8_t)(app_checksum >> 8); @@ -1111,6 +1242,26 @@ OREOLED::bootloader_fw_checksum(void) return _fw_checksum; } +int +OREOLED::bootloader_coerce_healthy(void) +{ + int ret = -1; + + /* check each unhealthy LED */ + /* this re-checks "unhealthy" LEDs as they can sometimes power up with the wrong ID, */ + /* but will have the correct ID once they jump to the application and be healthy again */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (!_healthy[i] && bootloader_app_ping(i) == OK) { + /* mark as healthy */ + _healthy[i] = true; + _num_healthy++; + ret = OK; + } + } + + return ret; +} + int OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) { From b4ea5efaa402220c47d8977b37c1f39652759c18 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Tue, 12 May 2015 10:05:33 +0800 Subject: [PATCH 145/293] oreoled: use version number from firmware binary --- src/drivers/drv_oreoled.h | 1 + src/drivers/oreoled/oreoled.cpp | 40 ++++++++++++++++----------------- 2 files changed, 20 insertions(+), 21 deletions(-) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 1f7911157006..380c12d88ab7 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -129,6 +129,7 @@ #define OREOLED_BOOT_CMD_PING_NONCE 0x2A #define OREOLED_BOOT_CMD_BOOT_NONCE 0xA2 +#define OREOLED_FW_FILE_HEADER_LENGTH 2 #define OREOLED_FW_FILE_SIZE_LIMIT 6144 #define OREOLED_FW_FILE "/etc/firmware/oreoled.bin" diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 1a152f046eb9..b2bee02257a7 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -384,17 +384,6 @@ OREOLED::cycle() } } - /* double check each unhealthy LED */ - /* this re-checks "unhealthy" LEDs as they can sometimes power up with the wrong ID, */ - /* but will have the correct ID once they jump to the application and be healthy again */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (!_healthy[i] && bootloader_app_ping(i) == OK) { - /* mark as healthy */ - _healthy[i] = true; - _num_healthy++; - } - } - /* coerce LEDs with startup issues to be healthy again */ bootloader_coerce_healthy(); @@ -963,8 +952,10 @@ OREOLED::bootloader_flash(int led_num) if (stat(OREOLED_FW_FILE, &s) != 0) return -1; + uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; + /* sanity-check file size */ - if (s.st_size > OREOLED_FW_FILE_SIZE_LIMIT) + if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) return -1; uint8_t *buf = new uint8_t[s.st_size]; @@ -977,10 +968,14 @@ OREOLED::bootloader_flash(int led_num) if (::read(fd, buf, s.st_size) != s.st_size) return -1; + /* Grab the version bytes from the binary */ + uint8_t version_major = buf[0]; + uint8_t version_minor = buf[1]; + ::close(fd); /* calculate flash pages (rounded up to nearest integer) */ - uint8_t flash_pages = ((s.st_size + 64 - 1) / 64); + uint8_t flash_pages = ((fw_length + 64 - 1) / 64); /* Set the current address */ set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); @@ -994,7 +989,7 @@ OREOLED::bootloader_flash(int led_num) memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_A; boot_cmd.buff[1] = page_idx; - memcpy(boot_cmd.buff+2, buf+(page_idx*64), 32); + memcpy(boot_cmd.buff+2, buf+(page_idx*64)+OREOLED_FW_FILE_HEADER_LENGTH, 32); boot_cmd.buff[32+2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 32+3; for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { @@ -1029,7 +1024,7 @@ OREOLED::bootloader_flash(int led_num) /* Send the second half of the 64 byte flash page */ memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_B; - memcpy(boot_cmd.buff+1, buf+(page_idx*64)+32, 32); + memcpy(boot_cmd.buff+1, buf+(page_idx*64)+32+OREOLED_FW_FILE_HEADER_LENGTH, 32); boot_cmd.buff[32+1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 32+2; for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { @@ -1073,7 +1068,7 @@ OREOLED::bootloader_flash(int led_num) /* Calculate a 16 bit XOR checksum of the flash */ /* Skip first two bytes which are modified by the bootloader */ uint16_t app_checksum = 0x0000; - for(uint16_t j = 2; j < s.st_size; j+=2) { + for(uint16_t j = 2+OREOLED_FW_FILE_HEADER_LENGTH; j < fw_length; j+=2) { app_checksum ^= (buf[j] << 8) | buf[j+1]; } warnx("bl finalise length = %i", s.st_size); @@ -1081,8 +1076,8 @@ OREOLED::bootloader_flash(int led_num) /* Flash writes must have succeeded so finalise the flash */ boot_cmd.buff[0] = OREOLED_BOOT_CMD_FINALISE_FLASH; - boot_cmd.buff[1] = *buf; /* First two bytes of the file buffer are the version number */ - boot_cmd.buff[2] = *buf+1; + boot_cmd.buff[1] = version_major; + boot_cmd.buff[2] = version_minor; boot_cmd.buff[3] = (uint8_t)(s.st_size >> 8); boot_cmd.buff[4] = (uint8_t)(s.st_size & 0xFF); boot_cmd.buff[5] = (uint8_t)(app_checksum >> 8); @@ -1210,8 +1205,10 @@ OREOLED::bootloader_fw_checksum(void) if (stat(OREOLED_FW_FILE, &s) != 0) return -1; + uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; + /* sanity-check file size */ - if (s.st_size > OREOLED_FW_FILE_SIZE_LIMIT) + if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) return -1; uint8_t *buf = new uint8_t[s.st_size]; @@ -1227,9 +1224,10 @@ OREOLED::bootloader_fw_checksum(void) ::close(fd); /* Calculate a 16 bit XOR checksum of the flash */ - /* Skip first two bytes which are modified by the bootloader */ + /* Skip the first two bytes which are the version information, plus + the next two bytes which are modified by the bootloader */ uint16_t app_checksum = 0x0000; - for(uint16_t j = 2; j < s.st_size; j+=2) { + for(uint16_t j = 2+OREOLED_FW_FILE_HEADER_LENGTH; j < s.st_size; j+=2) { app_checksum ^= (buf[j] << 8) | buf[j+1]; } warnx("bl finalise length = %i", s.st_size); From 13bdd11451e2b82684145df675034985b5047223 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Tue, 12 May 2015 10:20:31 +0800 Subject: [PATCH 146/293] oreoled: add ioctl to allow force sync of LEDs --- src/drivers/drv_oreoled.h | 3 +++ src/drivers/oreoled/oreoled.cpp | 4 ++++ 2 files changed, 7 insertions(+) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 380c12d88ab7..8be6eb8d1ac2 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -85,6 +85,9 @@ /** boot application */ #define OREOLED_BL_BOOT_APP _OREOLEDIOC(11) +/** force an i2c gencall */ +#define OREOLED_FORCE_SYNC _OREOLEDIOC(12) + /* Oreo LED driver supports up to 4 leds */ #define OREOLED_NUM_LEDS 4 diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index b2bee02257a7..ece438b87f71 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -1467,6 +1467,10 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; + case OREOLED_FORCE_SYNC: + send_general_call(); + break; + default: /* see if the parent class can make any use of it */ ret = CDev::ioctl(filp, cmd, arg); From f6ba8f34d6c731f0bcd9b5a10f689c96b5ec4668 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 14 May 2015 22:22:55 +0900 Subject: [PATCH 147/293] oreoled: formatting changes to match px4 style --- src/drivers/oreoled/oreoled.cpp | 361 +++++++++++++++++++------------- 1 file changed, 218 insertions(+), 143 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index ece438b87f71..96ccc26da9ab 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -266,7 +266,7 @@ OREOLED::info() } /* display perf info */ - perf_print_counter(_call_perf); + perf_print_counter(_call_perf); perf_print_counter(_gcall_perf); perf_print_counter(_probe_perf); perf_print_counter(_comms_errors); @@ -324,31 +324,35 @@ OREOLED::cycle() set_address(OREOLED_BASE_I2C_ADDR + i); /* Calculate XOR CRC and append to the i2c write data */ - msg[sizeof(msg)-1] = OREOLED_BASE_I2C_ADDR + i; - for(uint8_t j = 0; j < sizeof(msg)-1; j++) { - msg[sizeof(msg)-1] ^= msg[j]; + msg[sizeof(msg) - 1] = OREOLED_BASE_I2C_ADDR + i; + + for (uint8_t j = 0; j < sizeof(msg) - 1; j++) { + msg[sizeof(msg) - 1] ^= msg[j]; } /* send I2C command */ if (transfer(msg, sizeof(msg), reply, 3) == OK) { if (reply[1] == OREOLED_BASE_I2C_ADDR + i && - reply[2] == msg[sizeof(msg)-1]) { + reply[2] == msg[sizeof(msg) - 1]) { log("oreoled %u ok", (unsigned)i); _healthy[i] = true; _num_healthy++; /* If slaves are in application record that so we can reset if we need to bootload */ - if(bootloader_ping(i) == OK) { + if (bootloader_ping(i) == OK) { _in_boot[i] = true; _num_inboot++; } + } else { log("oreo reply errors: %u", (unsigned)_reply_errors); perf_count(_reply_errors); } + } else { perf_count(_comms_errors); } + perf_end(_probe_perf); } } @@ -357,14 +361,16 @@ OREOLED::cycle() work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_STARTUP_INTERVAL_US)); return; - } else if(_alwaysupdate) { + + } else if (_alwaysupdate) { /* reset each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { /* reset the LED if it's not in the bootloader */ /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ - if(!_in_boot[i]) + if (!_in_boot[i]) { bootloader_app_reset(i); + } } } @@ -394,13 +400,14 @@ OREOLED::cycle() work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); return; - } else if(_autoupdate) { + + } else if (_autoupdate) { /* check booted oreoleds to see if the app can report it's checksum (release versions >= v1.2) */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i] && !_in_boot[i]) { /* put any out of date oreoleds into bootloader mode */ /* being in bootloader mode signals to be code below that the will likey need updating */ - if(bootloader_inapp_checksum(i) != bootloader_fw_checksum()) { + if (bootloader_inapp_checksum(i) != bootloader_fw_checksum()) { bootloader_app_reset(i); } } @@ -409,7 +416,7 @@ OREOLED::cycle() /* reset all healthy oreoleds if the number of outdated oreoled's is > 0 */ /* this is done for consistency, so if only one oreoled is updating, all LEDs show the same behaviour */ /* otherwise a single oreoled could appear broken or failed. */ - if(_num_inboot > 0) { + if (_num_inboot > 0) { for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i] && !_in_boot[i]) { /* reset the LED if it's not in the bootloader */ @@ -422,7 +429,7 @@ OREOLED::cycle() for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i] && _in_boot[i]) { /* only flash LEDs with an old version of the applictioon */ - if(bootloader_app_checksum(i) != bootloader_fw_checksum()) { + if (bootloader_app_checksum(i) != bootloader_fw_checksum()) { /* flash the new firmware */ bootloader_flash(i); } @@ -448,11 +455,13 @@ OREOLED::cycle() work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); return; - } else if(_num_inboot > 0) { + + } else if (_num_inboot > 0) { /* boot any LEDs which are in still in bootloader mode */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_in_boot[i]) + if (_in_boot[i]) { bootloader_boot(i); + } } /* coerce LEDs with startup issues to be healthy again */ @@ -465,7 +474,8 @@ OREOLED::cycle() work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); return; - } else if(!_is_ready) { + + } else if (!_is_ready) { /* indicate a ready state since startup has finished */ _is_ready = true; } @@ -485,21 +495,24 @@ OREOLED::cycle() /* Calculate XOR CRC and append to the i2c write data */ uint8_t next_cmd_xor = OREOLED_BASE_I2C_ADDR + next_cmd.led_num; - for(uint8_t i = 0; i < next_cmd.num_bytes; i++) { + + for (uint8_t i = 0; i < next_cmd.num_bytes; i++) { next_cmd_xor ^= next_cmd.buff[i]; } + next_cmd.buff[next_cmd.num_bytes++] = next_cmd_xor; /* send I2C command with a retry limit */ - for(uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 3) == OK) { - if (reply[1] == (OREOLED_BASE_I2C_ADDR + next_cmd.led_num) && - reply[2] == next_cmd_xor) { + if (reply[1] == (OREOLED_BASE_I2C_ADDR + next_cmd.led_num) && reply[2] == next_cmd_xor) { /* slave returned a valid response */ break; + } else { perf_count(_reply_errors); } + } else { perf_count(_comms_errors); } @@ -539,17 +552,18 @@ OREOLED::bootloader_app_reset(int led_num) boot_cmd.buff[2] = OEROLED_RESET_NONCE; boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 4; - for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; } uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; /* send I2C command with a retry limit */ - for(uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && - reply[2] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { /* slave returned a valid response */ ret = OK; /* set this LED as being in boot mode now */ @@ -561,10 +575,10 @@ OREOLED::bootloader_app_reset(int led_num) } /* Allow time for the LED to reboot */ - usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); - usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); - usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); - usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); _is_bootloading = false; return ret; @@ -587,17 +601,18 @@ OREOLED::bootloader_app_ping(int led_num) boot_cmd.buff[2] = OREOLED_PATTERN_OFF; boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 4; - for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; } uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; /* send I2C command with a retry limit */ - for(uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && - reply[2] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { /* slave returned a valid response */ ret = OK; break; @@ -624,26 +639,28 @@ OREOLED::bootloader_inapp_checksum(int led_num) boot_cmd.buff[1] = OREOLED_PARAM_APP_CHECKSUM; boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 3; - for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; } uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - for(uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); /* Check the response */ - if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_PARAM_APP_CHECKSUM && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_PARAM_APP_CHECKSUM && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { warnx("bl app checksum OK from LED %i", boot_cmd.led_num); warnx("bl app checksum msb: 0x%x", reply[3]); warnx("bl app checksum lsb: 0x%x", reply[4]); ret = ((reply[3] << 8) | reply[4]); break; + } else { warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); warnx("bl app checksum response ADDR: 0x%x", reply[1]); @@ -651,8 +668,10 @@ OREOLED::bootloader_inapp_checksum(int led_num) warnx("bl app checksum response VER H: 0x%x", reply[3]); warnx("bl app checksum response VER L: 0x%x", reply[4]); warnx("bl app checksum response XOR: 0x%x", reply[5]); - if(retry > 1) { + + if (retry > 1) { warnx("bl app checksum retrying LED %i", boot_cmd.led_num); + } else { warnx("bl app checksum failed on LED %i", boot_cmd.led_num); break; @@ -672,40 +691,44 @@ OREOLED::bootloader_ping(int led_num) boot_cmd.led_num = led_num; int ret = -1; - + /* Set the current address */ set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); boot_cmd.buff[0] = OREOLED_BOOT_CMD_PING; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 2; - for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; } uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); /* Check the response */ - if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_PING && - reply[3] == OREOLED_BOOT_CMD_PING_NONCE && - reply[4] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_PING && + reply[3] == OREOLED_BOOT_CMD_PING_NONCE && + reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { warnx("bl ping OK from LED %i", boot_cmd.led_num); ret = OK; break; + } else { warnx("bl ping FAIL from LED %i", boot_cmd.led_num); warnx("bl ping response ADDR: 0x%x", reply[1]); warnx("bl ping response CMD: 0x%x", reply[2]); warnx("bl ping response NONCE: 0x%x", reply[3]); warnx("bl ping response XOR: 0x%x", reply[4]); - if(retry > 1) { + + if (retry > 1) { warnx("bl ping retrying LED %i", boot_cmd.led_num); + } else { warnx("bl ping failed on LED %i", boot_cmd.led_num); break; @@ -717,7 +740,7 @@ OREOLED::bootloader_ping(int led_num) return ret; } -uint8_t +uint8_t OREOLED::bootloader_version(int led_num) { _is_bootloading = true; @@ -732,32 +755,36 @@ OREOLED::bootloader_version(int led_num) boot_cmd.buff[0] = OREOLED_BOOT_CMD_BL_VER; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 2; - for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[k]; + + for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; } uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); /* Check the response */ - if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_BL_VER && - reply[3] == OREOLED_BOOT_SUPPORTED_VER && - reply[4] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BL_VER && + reply[3] == OREOLED_BOOT_SUPPORTED_VER && + reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { warnx("bl ver from LED %i = %i", boot_cmd.led_num, reply[3]); ret = reply[3]; break; + } else { warnx("bl ver response ADDR: 0x%x", reply[1]); warnx("bl ver response CMD: 0x%x", reply[2]); warnx("bl ver response VER: 0x%x", reply[3]); warnx("bl ver response XOR: 0x%x", reply[4]); - if(retry > 1) { + + if (retry > 1) { warnx("bl ver retrying LED %i", boot_cmd.led_num); + } else { warnx("bl ver failed on LED %i", boot_cmd.led_num); break; @@ -784,26 +811,28 @@ OREOLED::bootloader_app_version(int led_num) boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_VER; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 2; - for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; } uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); /* Check the response */ - if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_APP_VER && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_APP_VER && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { warnx("bl app version OK from LED %i", boot_cmd.led_num); warnx("bl app version msb: 0x%x", reply[3]); warnx("bl app version lsb: 0x%x", reply[4]); ret = ((reply[3] << 8) | reply[4]); break; + } else { warnx("bl app version FAIL from LED %i", boot_cmd.led_num); warnx("bl app version response ADDR: 0x%x", reply[1]); @@ -811,8 +840,10 @@ OREOLED::bootloader_app_version(int led_num) warnx("bl app version response VER H: 0x%x", reply[3]); warnx("bl app version response VER L: 0x%x", reply[4]); warnx("bl app version response XOR: 0x%x", reply[5]); - if(retry > 1) { + + if (retry > 1) { warnx("bl app version retrying LED %i", boot_cmd.led_num); + } else { warnx("bl app version failed on LED %i", boot_cmd.led_num); break; @@ -839,26 +870,28 @@ OREOLED::bootloader_app_checksum(int led_num) boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_CRC; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 2; - for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; } uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); /* Check the response */ - if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_APP_CRC && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_APP_CRC && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { warnx("bl app checksum OK from LED %i", boot_cmd.led_num); warnx("bl app checksum msb: 0x%x", reply[3]); warnx("bl app checksum lsb: 0x%x", reply[4]); ret = ((reply[3] << 8) | reply[4]); break; + } else { warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); warnx("bl app checksum response ADDR: 0x%x", reply[1]); @@ -866,8 +899,10 @@ OREOLED::bootloader_app_checksum(int led_num) warnx("bl app checksum response VER H: 0x%x", reply[3]); warnx("bl app checksum response VER L: 0x%x", reply[4]); warnx("bl app checksum response XOR: 0x%x", reply[5]); - if(retry > 1) { + + if (retry > 1) { warnx("bl app checksum retrying LED %i", boot_cmd.led_num); + } else { warnx("bl app checksum failed on LED %i", boot_cmd.led_num); break; @@ -896,31 +931,35 @@ OREOLED::bootloader_set_colour(int led_num, uint8_t red, uint8_t green) boot_cmd.buff[2] = green; boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 4; - for(uint8_t j = 0; j < boot_cmd.num_bytes-1; j++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[j]; + + for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; } uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); /* Check the response */ - if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_SET_COLOUR && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_SET_COLOUR && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { warnx("bl set colour OK from LED %i", boot_cmd.led_num); ret = OK; break; + } else { warnx("bl set colour FAIL from LED %i", boot_cmd.led_num); warnx("bl set colour response ADDR: 0x%x", reply[1]); warnx("bl set colour response CMD: 0x%x", reply[2]); warnx("bl set colour response XOR: 0x%x", reply[3]); - if(retry > 1) { + + if (retry > 1) { warnx("bl app colour retrying LED %i", boot_cmd.led_num); + } else { warnx("bl app colour failed on LED %i", boot_cmd.led_num); break; @@ -943,30 +982,35 @@ OREOLED::bootloader_flash(int led_num) int fd = ::open(OREOLED_FW_FILE, O_RDONLY); /* check for error opening the file */ - if (fd < 0) + if (fd < 0) { return -1; + } struct stat s; /* attempt to stat the file */ - if (stat(OREOLED_FW_FILE, &s) != 0) + if (stat(OREOLED_FW_FILE, &s) != 0) { return -1; + } uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; /* sanity-check file size */ - if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) + if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { return -1; + } uint8_t *buf = new uint8_t[s.st_size]; /* check that the buffer has been allocated */ - if (buf == NULL) + if (buf == NULL) { return -1; + } /* check that the firmware can be read into the buffer */ - if (::read(fd, buf, s.st_size) != s.st_size) + if (::read(fd, buf, s.st_size) != s.st_size) { return -1; + } /* Grab the version bytes from the binary */ uint8_t version_major = buf[0]; @@ -983,37 +1027,41 @@ OREOLED::bootloader_flash(int led_num) uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; /* Loop through flash pages */ - for(uint8_t page_idx = 0; page_idx < flash_pages; page_idx++) { + for (uint8_t page_idx = 0; page_idx < flash_pages; page_idx++) { /* Send the first half of the 64 byte flash page */ memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_A; boot_cmd.buff[1] = page_idx; - memcpy(boot_cmd.buff+2, buf+(page_idx*64)+OREOLED_FW_FILE_HEADER_LENGTH, 32); - boot_cmd.buff[32+2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 32+3; - for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[k]; + memcpy(boot_cmd.buff + 2, buf + (page_idx * 64) + OREOLED_FW_FILE_HEADER_LENGTH, 32); + boot_cmd.buff[32 + 2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 32 + 3; + + for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; } - for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); /* Check the response */ - if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_A && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_A && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { warnx("bl flash %ia OK for LED %i", page_idx, boot_cmd.led_num); break; + } else { warnx("bl flash %ia FAIL for LED %i", page_idx, boot_cmd.led_num); warnx("bl flash %ia response ADDR: 0x%x", page_idx, reply[1]); warnx("bl flash %ia response CMD: 0x%x", page_idx, reply[2]); warnx("bl flash %ia response XOR: 0x%x", page_idx, reply[3]); - if(retry > 1) { + + if (retry > 1) { warnx("bl flash %ia retrying LED %i", page_idx, boot_cmd.led_num); + } else { warnx("bl flash %ia failed on LED %i", page_idx, boot_cmd.led_num); return -1; @@ -1024,31 +1072,35 @@ OREOLED::bootloader_flash(int led_num) /* Send the second half of the 64 byte flash page */ memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_B; - memcpy(boot_cmd.buff+1, buf+(page_idx*64)+32+OREOLED_FW_FILE_HEADER_LENGTH, 32); - boot_cmd.buff[32+1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 32+2; - for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[k]; + memcpy(boot_cmd.buff + 1, buf + (page_idx * 64) + 32 + OREOLED_FW_FILE_HEADER_LENGTH, 32); + boot_cmd.buff[32 + 1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 32 + 2; + + for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; } - for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); /* Check the response */ - if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_B && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_B && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { warnx("bl flash %ib OK for LED %i", page_idx, boot_cmd.led_num); break; + } else { warnx("bl flash %ib FAIL for LED %i", page_idx, boot_cmd.led_num); warnx("bl flash %ib response ADDR: 0x%x", page_idx, reply[1]); warnx("bl flash %ib response CMD: 0x%x", page_idx, reply[2]); warnx("bl flash %ib response XOR: 0x%x", page_idx, reply[3]); - if(retry > 1) { + + if (retry > 1) { warnx("bl flash %ib retrying LED %i", page_idx, boot_cmd.led_num); + } else { errx(1, "bl flash %ib failed on LED %i", page_idx, boot_cmd.led_num); return -1; @@ -1058,19 +1110,22 @@ OREOLED::bootloader_flash(int led_num) /* Sleep to allow flash to write */ /* Wait extra long on the first write, to allow time for EEPROM updates */ - if(page_idx == 0) { - usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + if (page_idx == 0) { + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + } else { - usleep(OREOLED_BOOT_FLASH_WAITMS*1000); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000); } } /* Calculate a 16 bit XOR checksum of the flash */ /* Skip first two bytes which are modified by the bootloader */ uint16_t app_checksum = 0x0000; - for(uint16_t j = 2+OREOLED_FW_FILE_HEADER_LENGTH; j < fw_length; j+=2) { - app_checksum ^= (buf[j] << 8) | buf[j+1]; + + for (uint16_t j = 2 + OREOLED_FW_FILE_HEADER_LENGTH; j < fw_length; j += 2) { + app_checksum ^= (buf[j] << 8) | buf[j + 1]; } + warnx("bl finalise length = %i", s.st_size); warnx("bl finalise checksum = %i", app_checksum); @@ -1084,28 +1139,32 @@ OREOLED::bootloader_flash(int led_num) boot_cmd.buff[6] = (uint8_t)(app_checksum & 0xFF); boot_cmd.buff[7] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 8; - for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[k]; + + for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; } /* Try to finalise for twice the amount of normal retries */ - for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES*2; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES * 2; retry > 0; retry--) { /* Send the I2C Write */ memset(reply, 0, sizeof(reply)); transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); /* Check the response */ - if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_FINALISE_FLASH && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_FINALISE_FLASH && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { warnx("bl finalise OK from LED %i", boot_cmd.led_num); break; + } else { warnx("bl finalise response ADDR: 0x%x", reply[1]); warnx("bl finalise response CMD: 0x%x", reply[2]); warnx("bl finalise response XOR: 0x%x", reply[3]); - if(retry > 1) { + + if (retry > 1) { warnx("bl finalise retrying LED %i", boot_cmd.led_num); + } else { warnx("bl finalise failed on LED %i", boot_cmd.led_num); return -1; @@ -1114,8 +1173,8 @@ OREOLED::bootloader_flash(int led_num) } /* allow time for flash to finalise */ - usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); - usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); /* clean up file buffer */ delete buf; @@ -1132,7 +1191,7 @@ OREOLED::bootloader_boot(int led_num) boot_cmd.led_num = led_num; int ret = -1; - + /* Set the current address */ set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); @@ -1140,36 +1199,41 @@ OREOLED::bootloader_boot(int led_num) boot_cmd.buff[1] = OREOLED_BOOT_CMD_BOOT_NONCE; boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 3; - for(uint8_t k = 0; k < boot_cmd.num_bytes-1; k++) { - boot_cmd.buff[boot_cmd.num_bytes-1] ^= boot_cmd.buff[k]; + + for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { + boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; } - for(uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write */ uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); /* Check the response */ - if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_BOOT_APP && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BOOT_APP && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { warnx("bl boot OK from LED %i", boot_cmd.led_num); /* decrement the inboot counter so we don't get confused */ _in_boot[led_num] = false; _num_inboot--; ret = OK; break; - } else if(reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_BOOT_NONCE && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes-1]) { + + } else if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BOOT_NONCE && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { warnx("bl boot error from LED %i: no app", boot_cmd.led_num); break; + } else { warnx("bl boot response ADDR: 0x%x", reply[1]); warnx("bl boot response CMD: 0x%x", reply[2]); warnx("bl boot response XOR: 0x%x", reply[3]); - if(retry > 1) { + + if (retry > 1) { warnx("bl boot retrying LED %i", boot_cmd.led_num); + } else { warnx("bl boot failed on LED %i", boot_cmd.led_num); break; @@ -1178,10 +1242,10 @@ OREOLED::bootloader_boot(int led_num) } /* allow time for the LEDs to boot */ - usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); - usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); - usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); - usleep(OREOLED_BOOT_FLASH_WAITMS*1000*10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); _is_bootloading = false; return ret; @@ -1191,35 +1255,40 @@ uint16_t OREOLED::bootloader_fw_checksum(void) { /* Calculate the 16 bit XOR checksum of the firmware on the first call of this function */ - if(_fw_checksum == 0x0000) { + if (_fw_checksum == 0x0000) { /* Open the bootloader file */ int fd = ::open(OREOLED_FW_FILE, O_RDONLY); /* check for error opening the file */ - if (fd < 0) + if (fd < 0) { return -1; + } struct stat s; /* attempt to stat the file */ - if (stat(OREOLED_FW_FILE, &s) != 0) + if (stat(OREOLED_FW_FILE, &s) != 0) { return -1; + } uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; /* sanity-check file size */ - if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) + if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { return -1; + } uint8_t *buf = new uint8_t[s.st_size]; /* check that the buffer has been allocated */ - if (buf == NULL) + if (buf == NULL) { return -1; + } /* check that the firmware can be read into the buffer */ - if (::read(fd, buf, s.st_size) != s.st_size) + if (::read(fd, buf, s.st_size) != s.st_size) { return -1; + } ::close(fd); @@ -1227,9 +1296,11 @@ OREOLED::bootloader_fw_checksum(void) /* Skip the first two bytes which are the version information, plus the next two bytes which are modified by the bootloader */ uint16_t app_checksum = 0x0000; - for(uint16_t j = 2+OREOLED_FW_FILE_HEADER_LENGTH; j < s.st_size; j+=2) { - app_checksum ^= (buf[j] << 8) | buf[j+1]; + + for (uint16_t j = 2 + OREOLED_FW_FILE_HEADER_LENGTH; j < s.st_size; j += 2) { + app_checksum ^= (buf[j] << 8) | buf[j + 1]; } + warnx("bl finalise length = %i", s.st_size); warnx("bl finalise checksum = %i", app_checksum); @@ -1586,6 +1657,7 @@ oreoled_main(int argc, char *argv[]) /* handle autoupdate flag */ bool autoupdate = false; + if (argc > 2 && !strcmp(argv[2], "autoupdate")) { warnx("autoupdate enabled"); autoupdate = true; @@ -1593,6 +1665,7 @@ oreoled_main(int argc, char *argv[]) /* handle autoupdate flag */ bool alwaysupdate = false; + if (argc > 2 && !strcmp(argv[2], "alwaysupdate")) { warnx("alwaysupdate enabled"); alwaysupdate = true; @@ -1614,9 +1687,11 @@ oreoled_main(int argc, char *argv[]) } /* wait for up to 20 seconds for the driver become ready */ - for(uint8_t i = 0; i < 20; i++) { - if(g_oreoled->is_ready()) + for (uint8_t i = 0; i < 20; i++) { + if (g_oreoled->is_ready()) { break; + } + sleep(1); } @@ -1948,7 +2023,7 @@ oreoled_main(int argc, char *argv[]) } /* get bytes */ - sendb.num_bytes = argc - (optind+2); + sendb.num_bytes = argc - (optind + 2); uint8_t byte_count; for (byte_count = 0; byte_count < sendb.num_bytes; byte_count++) { From b67245eb9ff0f1330404485559677e62a6d706d9 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Tue, 26 May 2015 23:50:18 +1000 Subject: [PATCH 148/293] oreoled: fixed bug in firmware checksum calc This was causing he LEDs to boot on every power cycle. --- src/drivers/oreoled/oreoled.cpp | 68 ++++++++++----------------------- 1 file changed, 20 insertions(+), 48 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 96ccc26da9ab..c5d80371f5b8 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -35,7 +35,7 @@ /** * @file oreoled.cpp * - * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * Driver for oreoled ESCs found in solo, connected via I2C. * */ @@ -334,15 +334,19 @@ OREOLED::cycle() if (transfer(msg, sizeof(msg), reply, 3) == OK) { if (reply[1] == OREOLED_BASE_I2C_ADDR + i && reply[2] == msg[sizeof(msg) - 1]) { - log("oreoled %u ok", (unsigned)i); + log("oreoled %u ok - in bootloader", (unsigned)i); _healthy[i] = true; _num_healthy++; + _in_boot[i] = true; + _num_inboot++; - /* If slaves are in application record that so we can reset if we need to bootload */ - if (bootloader_ping(i) == OK) { - _in_boot[i] = true; - _num_inboot++; - } + } else if (reply[1] == OREOLED_BASE_I2C_ADDR + i && + reply[2] == msg[sizeof(msg) - 1]+1) { + log("oreoled %u ok - in application", (unsigned)i); + _healthy[i] = true; + _num_healthy++; + _healthy[i] = true; + _num_healthy++; } else { log("oreo reply errors: %u", (unsigned)_reply_errors); @@ -1118,23 +1122,14 @@ OREOLED::bootloader_flash(int led_num) } } - /* Calculate a 16 bit XOR checksum of the flash */ - /* Skip first two bytes which are modified by the bootloader */ - uint16_t app_checksum = 0x0000; - - for (uint16_t j = 2 + OREOLED_FW_FILE_HEADER_LENGTH; j < fw_length; j += 2) { - app_checksum ^= (buf[j] << 8) | buf[j + 1]; - } - - warnx("bl finalise length = %i", s.st_size); - warnx("bl finalise checksum = %i", app_checksum); + uint16_t app_checksum = bootloader_fw_checksum(); /* Flash writes must have succeeded so finalise the flash */ boot_cmd.buff[0] = OREOLED_BOOT_CMD_FINALISE_FLASH; boot_cmd.buff[1] = version_major; boot_cmd.buff[2] = version_minor; - boot_cmd.buff[3] = (uint8_t)(s.st_size >> 8); - boot_cmd.buff[4] = (uint8_t)(s.st_size & 0xFF); + boot_cmd.buff[3] = (uint8_t)(fw_length >> 8); + boot_cmd.buff[4] = (uint8_t)(fw_length & 0xFF); boot_cmd.buff[5] = (uint8_t)(app_checksum >> 8); boot_cmd.buff[6] = (uint8_t)(app_checksum & 0xFF); boot_cmd.buff[7] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; @@ -1301,8 +1296,8 @@ OREOLED::bootloader_fw_checksum(void) app_checksum ^= (buf[j] << 8) | buf[j + 1]; } - warnx("bl finalise length = %i", s.st_size); - warnx("bl finalise checksum = %i", app_checksum); + warnx("fw length = %i", fw_length); + warnx("fw checksum = %i", app_checksum); /* Store the checksum so it's only calculated once */ _fw_checksum = app_checksum; @@ -1421,8 +1416,6 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; case OREOLED_BL_PING: - _is_bootloading = true; - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { bootloader_ping(i); @@ -1430,12 +1423,9 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) } } - _is_bootloading = false; return ret; case OREOLED_BL_VER: - _is_bootloading = true; - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { bootloader_version(i); @@ -1443,12 +1433,9 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) } } - _is_bootloading = false; return ret; case OREOLED_BL_FLASH: - _is_bootloading = true; - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { bootloader_flash(i); @@ -1456,12 +1443,9 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) } } - _is_bootloading = false; return ret; case OREOLED_BL_APP_VER: - _is_bootloading = true; - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { bootloader_app_version(i); @@ -1469,12 +1453,9 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) } } - _is_bootloading = false; return ret; case OREOLED_BL_APP_CRC: - _is_bootloading = true; - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i]) { bootloader_app_checksum(i); @@ -1482,11 +1463,9 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) } } - _is_bootloading = false; return ret; case OREOLED_BL_SET_COLOUR: - _is_bootloading = true; new_cmd.led_num = OREOLED_ALL_INSTANCES; for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { @@ -1496,11 +1475,9 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) } } - _is_bootloading = false; return ret; case OREOLED_BL_BOOT_APP: - _is_bootloading = true; new_cmd.led_num = OREOLED_ALL_INSTANCES; for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { @@ -1510,7 +1487,6 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) } } - _is_bootloading = false; return ret; case OREOLED_SEND_BYTES: @@ -1655,18 +1631,14 @@ oreoled_main(int argc, char *argv[]) i2cdevice = PX4_I2C_BUS_LED; } - /* handle autoupdate flag */ + /* handle update flags */ bool autoupdate = false; + bool alwaysupdate = false; if (argc > 2 && !strcmp(argv[2], "autoupdate")) { warnx("autoupdate enabled"); autoupdate = true; - } - - /* handle autoupdate flag */ - bool alwaysupdate = false; - - if (argc > 2 && !strcmp(argv[2], "alwaysupdate")) { + } else if (argc > 2 && !strcmp(argv[2], "alwaysupdate")) { warnx("alwaysupdate enabled"); alwaysupdate = true; } @@ -1688,7 +1660,7 @@ oreoled_main(int argc, char *argv[]) /* wait for up to 20 seconds for the driver become ready */ for (uint8_t i = 0; i < 20; i++) { - if (g_oreoled->is_ready()) { + if (g_oreoled != nullptr && g_oreoled->is_ready()) { break; } From 1a897487ea9d8ee9b52386939be8da2c516d9dcb Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 28 May 2015 19:41:00 -0700 Subject: [PATCH 149/293] oreoled: fix double-increment of _num_healthy --- src/drivers/oreoled/oreoled.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index c5d80371f5b8..1d397d49b7da 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -345,8 +345,6 @@ OREOLED::cycle() log("oreoled %u ok - in application", (unsigned)i); _healthy[i] = true; _num_healthy++; - _healthy[i] = true; - _num_healthy++; } else { log("oreo reply errors: %u", (unsigned)_reply_errors); From 4a038c2d87b37b814d323950d5df68fad50488c9 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Sun, 31 May 2015 23:46:04 +1000 Subject: [PATCH 150/293] oreoled: fix backwards compatability --- src/drivers/oreoled/oreoled.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 1d397d49b7da..fc506f18335d 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -337,11 +337,19 @@ OREOLED::cycle() log("oreoled %u ok - in bootloader", (unsigned)i); _healthy[i] = true; _num_healthy++; - _in_boot[i] = true; - _num_inboot++; + /* If slaves are in application record that so we can reset if we need to bootload */ + /* This additional check is required for LED firmwares below v1.3 and can be + deprecated once all LEDs in the wild have firmware >= v1.3 */ + if(bootloader_ping(i) == OK) { + _in_boot[i] = true; + _num_inboot++; + } + + /* Check for a reply with a checksum offset of 1, + which indicates a response from firmwares >= 1.3 */ } else if (reply[1] == OREOLED_BASE_I2C_ADDR + i && - reply[2] == msg[sizeof(msg) - 1]+1) { + reply[2] == msg[sizeof(msg) - 1] + 1) { log("oreoled %u ok - in application", (unsigned)i); _healthy[i] = true; _num_healthy++; @@ -367,18 +375,16 @@ OREOLED::cycle() } else if (_alwaysupdate) { /* reset each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { + if (_healthy[i] && !_in_boot[i]) { /* reset the LED if it's not in the bootloader */ /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ - if (!_in_boot[i]) { - bootloader_app_reset(i); - } + bootloader_app_reset(i); } } /* attempt to update each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { + if (_healthy[i] && _in_boot[i]) { /* flash the new firmware */ bootloader_flash(i); } @@ -386,7 +392,7 @@ OREOLED::cycle() /* boot each healthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { + if (_healthy[i] && _in_boot[i]) { /* boot the application */ bootloader_boot(i); } From b39d9ae4e1c33f4f1757c1d0e2413390f29ab2ee Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Mon, 1 Jun 2015 13:32:06 +1000 Subject: [PATCH 151/293] oreoled: close file handles and free memory --- src/drivers/oreoled/oreoled.cpp | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index fc506f18335d..b6095aa6b9ed 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -998,6 +998,7 @@ OREOLED::bootloader_flash(int led_num) /* attempt to stat the file */ if (stat(OREOLED_FW_FILE, &s) != 0) { + ::close(fd); return -1; } @@ -1005,6 +1006,7 @@ OREOLED::bootloader_flash(int led_num) /* sanity-check file size */ if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { + ::close(fd); return -1; } @@ -1012,20 +1014,23 @@ OREOLED::bootloader_flash(int led_num) /* check that the buffer has been allocated */ if (buf == NULL) { + ::close(fd); return -1; } /* check that the firmware can be read into the buffer */ if (::read(fd, buf, s.st_size) != s.st_size) { + ::close(fd); + delete[] buf; return -1; } + ::close(fd); + /* Grab the version bytes from the binary */ uint8_t version_major = buf[0]; uint8_t version_minor = buf[1]; - ::close(fd); - /* calculate flash pages (rounded up to nearest integer) */ uint8_t flash_pages = ((fw_length + 64 - 1) / 64); @@ -1072,6 +1077,7 @@ OREOLED::bootloader_flash(int led_num) } else { warnx("bl flash %ia failed on LED %i", page_idx, boot_cmd.led_num); + delete[] buf; return -1; } } @@ -1111,6 +1117,7 @@ OREOLED::bootloader_flash(int led_num) } else { errx(1, "bl flash %ib failed on LED %i", page_idx, boot_cmd.led_num); + delete[] buf; return -1; } } @@ -1166,6 +1173,7 @@ OREOLED::bootloader_flash(int led_num) } else { warnx("bl finalise failed on LED %i", boot_cmd.led_num); + delete[] buf; return -1; } } @@ -1176,7 +1184,7 @@ OREOLED::bootloader_flash(int led_num) usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); /* clean up file buffer */ - delete buf; + delete[] buf; _is_bootloading = false; return 1; @@ -1267,6 +1275,7 @@ OREOLED::bootloader_fw_checksum(void) /* attempt to stat the file */ if (stat(OREOLED_FW_FILE, &s) != 0) { + ::close(fd); return -1; } @@ -1274,6 +1283,7 @@ OREOLED::bootloader_fw_checksum(void) /* sanity-check file size */ if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { + ::close(fd); return -1; } @@ -1281,11 +1291,14 @@ OREOLED::bootloader_fw_checksum(void) /* check that the buffer has been allocated */ if (buf == NULL) { + ::close(fd); return -1; } /* check that the firmware can be read into the buffer */ if (::read(fd, buf, s.st_size) != s.st_size) { + ::close(fd); + delete[] buf; return -1; } @@ -1300,6 +1313,8 @@ OREOLED::bootloader_fw_checksum(void) app_checksum ^= (buf[j] << 8) | buf[j + 1]; } + delete[] buf; + warnx("fw length = %i", fw_length); warnx("fw checksum = %i", app_checksum); From 0b13186df6c3cc713081478f25a65ed0e5ba90ac Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 21 Jun 2015 16:29:07 +1000 Subject: [PATCH 152/293] px4iofirmware: blink blue LED more rapidly when override is active this makes it easier for a user to test that override is working correctly in pre-flight checks. Otherwise override is hard to distinguish from normal manual mode --- src/modules/px4iofirmware/px4io.c | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 94f1fc11ef5d..9f8c3b526d74 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -354,7 +354,18 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); - if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { + /* + blink blue LED at 4Hz in normal operation. When in + override blink 4x faster so the user can clearly see + that override is happening. This helps when + pre-flight testing the override system + */ + uint32_t heartbeat_period_us = 250*1000UL; + if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { + heartbeat_period_us /= 4; + } + + if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) { last_heartbeat_time = hrt_absolute_time(); heartbeat_blink(); } From 005080d77f91b134de8ed67e8d2f69a38d454820 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 21 Jun 2015 18:02:31 +1000 Subject: [PATCH 153/293] px4iofirmware: allow override when RAW_PWM is active if override is enabled then it should apply even if RAW_PWM is being supplied by the FMU --- src/modules/px4iofirmware/mixer.cpp | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index eaff89e7c275..4fc0fe938a6f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -115,13 +115,23 @@ mixer_tick(void) * Decide which set of controls we're using. */ + bool override_enabled = ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)); + /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - /* don't actually mix anything - we already have raw PWM values */ - source = MIX_NONE; - + if (override_enabled) { + /* a channel based override has been + * triggered, with FMU active */ + source = MIX_OVERRIDE_FMU_OK; + } else { + /* don't actually mix anything - we already have raw PWM values */ + source = MIX_NONE; + } } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && @@ -132,21 +142,15 @@ mixer_tick(void) source = MIX_FMU; } - if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + if ( override_enabled && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && /* do not enter manual override if we asked for termination failsafe and FMU is lost */ !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; - } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + } else if ( override_enabled && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly up to available rc channels */ source = MIX_OVERRIDE_FMU_OK; From 2120816ed67e5d809746038ed3ea5dfc4d0a606c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 23 Jun 2015 14:27:22 +1000 Subject: [PATCH 154/293] APM: ignore abother generated file --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 01f693adb03c..dddebfe3a452 100644 --- a/.gitignore +++ b/.gitignore @@ -51,4 +51,5 @@ xcode src/platforms/posix/px4_messages/ src/platforms/qurt/px4_messages/ makefiles/config_px4fmu-v2_APM.mk +makefiles/nuttx/config_px4fmu-v1_APM.mk makefiles/nuttx/config_px4fmu-v2_APM.mk From b0cb142b116fadd86b1d8be3ccb3c670a0311a93 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 27 Jun 2015 21:23:53 +1000 Subject: [PATCH 155/293] ms5611: fixed the i2c device on NuttX this may well break the posix PX4 port. I'll talk to Mark about how to fix --- src/drivers/ms5611/ms5611_i2c.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 730fd9b96899..11baafd23bf3 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -71,8 +71,8 @@ class MS5611_I2C : public device::I2C virtual ~MS5611_I2C(); virtual int init(); - virtual int dev_read(unsigned offset, void *data, unsigned count); - virtual int dev_ioctl(unsigned operation, unsigned &arg); + virtual int read(unsigned offset, void *data, unsigned count); + virtual int ioctl(unsigned operation, unsigned &arg); #ifdef __PX4_NUTTX protected: @@ -139,7 +139,7 @@ MS5611_I2C::init() } int -MS5611_I2C::dev_read(unsigned offset, void *data, unsigned count) +MS5611_I2C::read(unsigned offset, void *data, unsigned count) { union _cvt { uint8_t b[4]; @@ -162,7 +162,7 @@ MS5611_I2C::dev_read(unsigned offset, void *data, unsigned count) } int -MS5611_I2C::dev_ioctl(unsigned operation, unsigned &arg) +MS5611_I2C::ioctl(unsigned operation, unsigned &arg) { int ret; From 55d4ea7a9d5586d6fb4d4633bb8df7b71b550980 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 7 Aug 2015 15:43:40 +1000 Subject: [PATCH 156/293] pwm_input: added perf counters and remove console output perf counters are preferable to printf warnings --- src/drivers/pwm_input/pwm_input.cpp | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index d06d125e5803..800bc85ba093 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -80,6 +80,8 @@ #include #include +#include + #include #include #include @@ -254,6 +256,10 @@ class PWMIN : device::CDev hrt_call _hard_reset_call; /* HRT callout for note completion */ hrt_call _freeze_test_call; /* HRT callout for note completion */ + perf_counter_t _perf_reset; + perf_counter_t _perf_interrupt; + perf_counter_t _perf_read; + void _timer_init(void); void _turn_on(); @@ -277,7 +283,10 @@ PWMIN::PWMIN() : _last_period(0), _last_width(0), _reports(nullptr), - _timer_started(false) + _timer_started(false), + _perf_reset(perf_alloc(PC_COUNT, "pwm_input_reset")), + _perf_read(perf_alloc(PC_ELAPSED, "pwm_input_read")), + _perf_interrupt(perf_alloc(PC_ELAPSED, "pwm_input_interrupt")) { } @@ -286,6 +295,9 @@ PWMIN::~PWMIN() if (_reports != nullptr) { delete _reports; } + perf_free(_perf_reset); + perf_free(_perf_read); + perf_free(_perf_interrupt); } /* @@ -368,6 +380,8 @@ void PWMIN::_timer_init(void) irqrestore(flags); _timer_started = true; + + perf_count(_perf_reset); } void @@ -375,7 +389,6 @@ PWMIN::_freeze_test() { /* reset if last poll time was way back and a read was recently requested */ if (hrt_elapsed_time(&_last_poll_time) > TIMEOUT_POLL && hrt_elapsed_time(&_last_read_time) < TIMEOUT_READ) { - warnx("Lidar is down, reseting"); hard_reset(); } } @@ -470,6 +483,8 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t PWMIN::read(struct file *filp, char *buffer, size_t buflen) { + perf_begin(_perf_read); + _last_read_time = hrt_absolute_time(); unsigned count = buflen / sizeof(struct pwm_input_s); @@ -478,6 +493,7 @@ PWMIN::read(struct file *filp, char *buffer, size_t buflen) /* buffer must be large enough */ if (count < 1) { + perf_end(_perf_read); return -ENOSPC; } @@ -487,6 +503,9 @@ PWMIN::read(struct file *filp, char *buffer, size_t buflen) buf++; } } + + perf_end(_perf_read); + /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } @@ -496,6 +515,8 @@ PWMIN::read(struct file *filp, char *buffer, size_t buflen) */ void PWMIN::publish(uint16_t status, uint32_t period, uint32_t pulse_width) { + perf_count(_perf_interrupt); + /* if we missed an edge, we have to give up */ if (status & SR_OVF_PWMIN) { _error_count++; @@ -526,6 +547,9 @@ void PWMIN::print_info(void) (unsigned)_pulses_captured, (unsigned)_last_period, (unsigned)_last_width); + perf_print_counter(_perf_interrupt); + perf_print_counter(_perf_read); + perf_print_counter(_perf_reset); } } From 6d634d23188eb4a99c89201fcb00be42ced10ed9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Aug 2015 14:53:24 +1000 Subject: [PATCH 157/293] ms5611: cleanup usage message --- src/drivers/ms5611/ms5611_nuttx.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index 87036cd2e19d..2e98de198292 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -1140,10 +1140,10 @@ calibrate(unsigned altitude, enum MS5611_BUS busid) void usage() { - warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'calibrate'"); warnx("options:"); warnx(" -X (external I2C bus)"); - warnx(" -I (intternal I2C bus)"); + warnx(" -I (internal I2C bus)"); warnx(" -S (external SPI bus)"); warnx(" -s (internal SPI bus)"); } @@ -1215,5 +1215,6 @@ ms5611_main(int argc, char *argv[]) ms5611::calibrate(altitude, busid); } - errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); + ms5611::usage(); + errx(1, "unrecognised command"); } From 7a620546f08dbfe9033853c2c29e8d8bae9970e5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 15 Aug 2015 19:13:07 +1000 Subject: [PATCH 158/293] ll40ls: added support for 'blue label' lidars the key is splitting up read_reg() into two transfers --- src/drivers/ll40ls/LidarLiteI2C.cpp | 84 ++++++++++++++++++----------- src/drivers/ll40ls/LidarLiteI2C.h | 24 +++++++-- 2 files changed, 72 insertions(+), 36 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index f3fb0dda45c6..c6190aed50bc 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -72,6 +72,8 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : _zero_counter(0), _acquire_time_usec(0), _pause_measurements(false), + _hw_version(0), + _sw_version(0), _bus(bus) { // up the retries since the device misses the first measure attempts @@ -146,7 +148,30 @@ int LidarLiteI2C::init() int LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val) { - return transfer(®, 1, &val, 1); + return lidar_transfer(®, 1, &val, 1); +} + +int LidarLiteI2C::write_reg(uint8_t reg, uint8_t val) +{ + const uint8_t cmd[2] = { reg, val }; + return transfer(&cmd[0], 2, NULL, 0); +} + +/* + LidarLite specific transfer() function that avoids a stop condition + with SCL low + */ +int LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + if (send != NULL && send_len > 0) { + int ret = transfer(send, send_len, NULL, 0); + if (ret != OK) + return ret; + } + if (recv != NULL && recv_len > 0) { + return transfer(NULL, 0, recv, recv_len); + } + return OK; } int LidarLiteI2C::probe() @@ -158,28 +183,18 @@ int LidarLiteI2C::probe() _retries = 10; for (uint8_t i = 0; i < sizeof(addresses); i++) { - uint8_t who_am_i = 0, max_acq_count = 0; - - // set the I2C bus address - set_address(addresses[i]); - - /* register 2 defaults to 0x80. If this matches it is - almost certainly a ll40ls */ - if (read_reg(LL40LS_MAX_ACQ_COUNT_REG, max_acq_count) == OK && max_acq_count == 0x80) { - // very likely to be a ll40ls. This is the - // default max acquisition counter - goto ok; - } - - if (read_reg(LL40LS_WHO_AM_I_REG, who_am_i) == OK && who_am_i == LL40LS_WHO_AM_I_REG_VAL) { - // it is responding correctly to a - // WHO_AM_I. This works with older sensors (pre-production) + /* + check for hw and sw versions. It would be better if + we had a proper WHOAMI register + */ + if (read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version > 0 && + read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0) { goto ok; } - debug("probe failed reg11=0x%02x reg2=0x%02x\n", - (unsigned)who_am_i, - (unsigned)max_acq_count); + debug("probe failed hw_version=0x%02x sw_version=0x%02x\n", + (unsigned)_hw_version, + (unsigned)_sw_version); } // not found on any address @@ -187,9 +202,6 @@ int LidarLiteI2C::probe() ok: _retries = 3; - - // reset the sensor to ensure it is in a known state with - // correct settings return reset_sensor(); } @@ -303,7 +315,7 @@ int LidarLiteI2C::measure() * Send the command to begin a measurement. */ const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; - ret = transfer(cmd, sizeof(cmd), nullptr, 0); + ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0); if (OK != ret) { perf_count(_comms_errors); @@ -332,9 +344,14 @@ int LidarLiteI2C::measure() */ int LidarLiteI2C::reset_sensor() { - const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET }; - int ret = transfer(cmd, sizeof(cmd), nullptr, 0); - return ret; + int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); + if (ret != OK) + return ret; + + // wait for sensor reset to complete + usleep(1000); + + return OK; } /* @@ -349,7 +366,7 @@ void LidarLiteI2C::print_registers() for (uint8_t reg = 0; reg <= 0x67; reg++) { uint8_t val = 0; - int ret = transfer(®, 1, &val, 1); + int ret = lidar_transfer(®, 1, &val, 1); if (ret != OK) { printf("%02x:XX ", (unsigned)reg); @@ -377,10 +394,12 @@ int LidarLiteI2C::collect() perf_begin(_sample_perf); // read the high and low byte distance registers - uint8_t distance_reg = LL40LS_DISTHIGH_REG; - ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); + uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT; + ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val)); - if (ret < 0) { + // if the transfer failed or if the high bit of distance is + // set then the distance is invalid + if (ret < 0 || (val[0] & 0x80)) { if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { /* NACKs from the sensor are expected when we @@ -427,7 +446,8 @@ int LidarLiteI2C::collect() _zero_counter = 0; } - _last_distance = distance_m; + _last_distance = distance_cm; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index d9218e34351c..51cca0862f5a 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -63,10 +63,10 @@ #define LL40LS_MEASURE_REG 0x00 /* Measure range register */ #define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */ #define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ -#define LL40LS_MAX_ACQ_COUNT_REG 0x02 /* maximum acquisition count register */ -#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ -#define LL40LS_WHO_AM_I_REG 0x11 -#define LL40LS_WHO_AM_I_REG_VAL 0xCA +#define LL40LS_DISTHIGH_REG 0x0F /* High byte of distance register, auto increment */ +#define LL40LS_AUTO_INCREMENT 0x80 +#define LL40LS_HW_VERSION 0x41 +#define LL40LS_SW_VERSION 0x4f #define LL40LS_SIGNAL_STRENGTH_REG 0x5b class LidarLiteI2C : public LidarLite, public device::I2C @@ -93,6 +93,7 @@ class LidarLiteI2C : public LidarLite, public device::I2C protected: int probe(); int read_reg(uint8_t reg, uint8_t &val); + int write_reg(uint8_t reg, uint8_t val); int measure() override; int reset_sensor(); @@ -116,10 +117,25 @@ class LidarLiteI2C : public LidarLite, public device::I2C uint16_t _zero_counter; uint64_t _acquire_time_usec; volatile bool _pause_measurements; + uint8_t _hw_version; + uint8_t _sw_version; /**< the bus the device is connected to */ int _bus; + /** + * LidarLite specific transfer function. This is needed + * to avoid a stop transition with SCL high + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @param recv Pointer to buffer for bytes received. + * @param recv_len Number of bytes to receive. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); + /** * Test whether the device supported by the driver is present at a * specific address. From 5746c40a69f269f80c7fd95ebb45d77c0c9a3d26 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 1 Aug 2015 16:32:55 +1000 Subject: [PATCH 159/293] mpu9250: first cut at a MPU9250 driver --- src/drivers/mpu9250/module.mk | 46 + src/drivers/mpu9250/mpu9250.cpp | 2062 +++++++++++++++++++++++++++++++ 2 files changed, 2108 insertions(+) create mode 100644 src/drivers/mpu9250/module.mk create mode 100644 src/drivers/mpu9250/mpu9250.cpp diff --git a/src/drivers/mpu9250/module.mk b/src/drivers/mpu9250/module.mk new file mode 100644 index 000000000000..80ee992da48a --- /dev/null +++ b/src/drivers/mpu9250/module.mk @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the MPU6000 driver. +# + +MODULE_COMMAND = mpu9250 + +SRCS = mpu9250.cpp + +MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp new file mode 100644 index 000000000000..61dc1673b5df --- /dev/null +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -0,0 +1,2062 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 mpu9250.cpp + * + * Driver for the Invensense MPU9250 connected via SPI. + * + * @author Andrew Tridgell + * + * based on the mpu6000 driver + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#define DIR_READ 0x80 +#define DIR_WRITE 0x00 + +#define MPU_DEVICE_PATH_ACCEL "/dev/mpu9250_accel" +#define MPU_DEVICE_PATH_GYRO "/dev/mpu9250_gyro" +#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu9250_accel_ext" +#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu9250_gyro_ext" + +// MPU 9250 registers +#define MPUREG_WHOAMI 0x75 +#define MPUREG_SMPLRT_DIV 0x19 +#define MPUREG_CONFIG 0x1A +#define MPUREG_GYRO_CONFIG 0x1B +#define MPUREG_ACCEL_CONFIG 0x1C +#define MPUREG_ACCEL_CONFIG2 0x1D +#define MPUREG_LPACCEL_ODR 0x1E +#define MPUREG_WOM_THRESH 0x1F +#define MPUREG_FIFO_EN 0x23 +#define MPUREG_I2C_MST_CTRL 0x24 +#define MPUREG_I2C_SLV0_ADDR 0x25 +#define MPUREG_I2C_SLV0_REG 0x26 +#define MPUREG_I2C_SLV0_CTRL 0x27 +#define MPUREG_I2C_SLV1_ADDR 0x28 +#define MPUREG_I2C_SLV1_REG 0x29 +#define MPUREG_I2C_SLV1_CTRL 0x2A +#define MPUREG_I2C_SLV2_ADDR 0x2B +#define MPUREG_I2C_SLV2_REG 0x2C +#define MPUREG_I2C_SLV2_CTRL 0x2D +#define MPUREG_I2C_SLV3_ADDR 0x2E +#define MPUREG_I2C_SLV3_REG 0x2F +#define MPUREG_I2C_SLV3_CTRL 0x30 +#define MPUREG_I2C_SLV4_ADDR 0x31 +#define MPUREG_I2C_SLV4_REG 0x32 +#define MPUREG_I2C_SLV4_DO 0x33 +#define MPUREG_I2C_SLV4_CTRL 0x34 +#define MPUREG_I2C_SLV4_DI 0x35 +#define MPUREG_I2C_MST_STATUS 0x36 +#define MPUREG_INT_PIN_CFG 0x37 +#define MPUREG_INT_ENABLE 0x38 +#define MPUREG_INT_STATUS 0x3A +#define MPUREG_ACCEL_XOUT_H 0x3B +#define MPUREG_ACCEL_XOUT_L 0x3C +#define MPUREG_ACCEL_YOUT_H 0x3D +#define MPUREG_ACCEL_YOUT_L 0x3E +#define MPUREG_ACCEL_ZOUT_H 0x3F +#define MPUREG_ACCEL_ZOUT_L 0x40 +#define MPUREG_TEMP_OUT_H 0x41 +#define MPUREG_TEMP_OUT_L 0x42 +#define MPUREG_GYRO_XOUT_H 0x43 +#define MPUREG_GYRO_XOUT_L 0x44 +#define MPUREG_GYRO_YOUT_H 0x45 +#define MPUREG_GYRO_YOUT_L 0x46 +#define MPUREG_GYRO_ZOUT_H 0x47 +#define MPUREG_GYRO_ZOUT_L 0x48 +#define MPUREG_EXT_SENS_DATA_00 0x49 +#define MPUREG_I2C_SLV0_D0 0x63 +#define MPUREG_I2C_SLV1_D0 0x64 +#define MPUREG_I2C_SLV2_D0 0x65 +#define MPUREG_I2C_SLV3_D0 0x66 +#define MPUREG_I2C_MST_DELAY_CTRL 0x67 +#define MPUREG_SIGNAL_PATH_RESET 0x68 +#define MPUREG_MOT_DETECT_CTRL 0x69 +#define MPUREG_USER_CTRL 0x6A +#define MPUREG_PWR_MGMT_1 0x6B +#define MPUREG_PWR_MGMT_2 0x6C +#define MPUREG_FIFO_COUNTH 0x72 +#define MPUREG_FIFO_COUNTL 0x73 +#define MPUREG_FIFO_R_W 0x74 + +// Configuration bits MPU 9250 +#define BIT_SLEEP 0x40 +#define BIT_H_RESET 0x80 +#define MPU_CLK_SEL_AUTO 0x01 + +#define BITS_GYRO_ST_X 0x80 +#define BITS_GYRO_ST_Y 0x40 +#define BITS_GYRO_ST_Z 0x20 +#define BITS_FS_250DPS 0x00 +#define BITS_FS_500DPS 0x08 +#define BITS_FS_1000DPS 0x10 +#define BITS_FS_2000DPS 0x18 +#define BITS_FS_MASK 0x18 + +#define BITS_DLPF_CFG_250HZ 0x00 +#define BITS_DLPF_CFG_184HZ 0x01 +#define BITS_DLPF_CFG_92HZ 0x02 +#define BITS_DLPF_CFG_41HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BITS_DLPF_CFG_3600HZ 0x07 +#define BITS_DLPF_CFG_MASK 0x07 + +#define BIT_RAW_RDY_EN 0x01 +#define BIT_INT_ANYRD_2CLEAR 0x10 + +#define MPU_WHOAMI_9250 0x71 + +#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41 +#define MPU9250_ACCEL_DEFAULT_RATE 1000 +#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define MPU9250_GYRO_DEFAULT_RATE 1000 +#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU9250_ONE_G 9.80665f + +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + +/* + the MPU9250 can only handle high SPI bus speeds on the sensor and + interrupt status registers. All other registers have a maximum 1MHz + SPI speed + */ +#define MPU9250_LOW_BUS_SPEED 1000*1000 +#define MPU9250_HIGH_BUS_SPEED 11*1000*1000 + +/* + we set the timer interrupt to run a bit faster than the desired + sample rate and then throw away duplicates by comparing + accelerometer values. This time reduction is enough to cope with + worst case timing jitter due to other timers + */ +#define MPU9250_TIMER_REDUCTION 200 + +class MPU9250_gyro; + +class MPU9250 : public device::SPI +{ +public: + MPU9250(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation); + virtual ~MPU9250(); + + 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(); + + void print_registers(); + + // deliberately cause a sensor error + void test_error(); + +protected: + virtual int probe(); + + friend class MPU9250_gyro; + + virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen); + virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + MPU9250_gyro *_gyro; + uint8_t _whoami; /** whoami result */ + + struct hrt_call _call; + unsigned _call_interval; + + ringbuffer::RingBuffer *_accel_reports; + + struct accel_scale _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + orb_advert_t _accel_topic; + int _accel_orb_class_instance; + int _accel_class_instance; + + ringbuffer::RingBuffer *_gyro_reports; + + struct gyro_scale _gyro_scale; + float _gyro_range_scale; + float _gyro_range_rad_s; + + unsigned _dlpf_freq; + + unsigned _sample_rate; + perf_counter_t _accel_reads; + perf_counter_t _gyro_reads; + perf_counter_t _sample_perf; + perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; + perf_counter_t _good_transfers; + perf_counter_t _reset_retries; + perf_counter_t _duplicates; + perf_counter_t _system_latency_perf; + perf_counter_t _controller_latency_perf; + + uint8_t _register_wait; + uint64_t _reset_wait; + + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + + enum Rotation _rotation; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define MPU9250_NUM_CHECKED_REGISTERS 11 + static const uint8_t _checked_registers[MPU9250_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS]; + uint8_t _checked_bad[MPU9250_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + + // last temperature reading for print_info() + float _last_temperature; + + // keep last accel reading for duplicate detection + uint16_t _last_accel[3]; + bool _got_duplicate; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + int reset(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report buffers. + */ + void measure(); + + /** + * Read a register from the MPU9250 + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg, uint32_t speed=MPU9250_LOW_BUS_SPEED); + uint16_t read_reg16(unsigned reg); + + /** + * Write a register in the MPU9250 + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the MPU9250 + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Write a register in the MPU9250, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** + * Set the MPU9250 measurement range. + * + * @param max_g The maximum G value the range must support. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_accel_range(unsigned max_g); + + /** + * Swap a 16-bit value read from the MPU9250 to native byte order. + */ + uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } + + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + + /** + * Measurement self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); + + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Gyro self test + * + * @return 0 on success, 1 on failure + */ + int gyro_self_test(); + + /* + set low pass filter frequency + */ + void _set_dlpf_filter(uint16_t frequency_hz); + + /* + set sample rate (approximate) - 1kHz to 5Hz + */ + void _set_sample_rate(unsigned desired_sample_rate_hz); + + /* + check that key registers still have the right value + */ + void check_registers(void); + + /* do not allow to copy this class due to pointer data members */ + MPU9250(const MPU9250&); + MPU9250 operator=(const MPU9250&); + +#pragma pack(push, 1) + /** + * Report conversation within the MPU9250, including command byte and + * interrupt status. + */ + struct MPUReport { + uint8_t cmd; + uint8_t status; + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + }; +#pragma pack(pop) +}; + +/* + list of registers that will be checked in check_registers(). Note + that MPUREG_PRODUCT_ID must be first in the list. + */ +const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPUREG_WHOAMI, + MPUREG_PWR_MGMT_1, + MPUREG_PWR_MGMT_2, + MPUREG_USER_CTRL, + MPUREG_SMPLRT_DIV, + MPUREG_CONFIG, + MPUREG_GYRO_CONFIG, + MPUREG_ACCEL_CONFIG, + MPUREG_ACCEL_CONFIG2, + MPUREG_INT_ENABLE, + MPUREG_INT_PIN_CFG }; + + + +/** + * Helper class implementing the gyro driver node. + */ +class MPU9250_gyro : public device::CDev +{ +public: + MPU9250_gyro(MPU9250 *parent, const char *path); + ~MPU9250_gyro(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + virtual int init(); + +protected: + friend class MPU9250; + + void parent_poll_notify(); + +private: + MPU9250 *_parent; + orb_advert_t _gyro_topic; + int _gyro_orb_class_instance; + int _gyro_class_instance; + + /* do not allow to copy this class due to pointer data members */ + MPU9250_gyro(const MPU9250_gyro&); + MPU9250_gyro operator=(const MPU9250_gyro&); +}; + +/** driver 'main' command */ +extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); } + +MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) : + SPI("MPU9250", path_accel, bus, device, SPIDEV_MODE3, MPU9250_LOW_BUS_SPEED), + _gyro(new MPU9250_gyro(this, path_gyro)), + _whoami(0), + _call{}, + _call_interval(0), + _accel_reports(nullptr), + _accel_scale{}, + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(nullptr), + _accel_orb_class_instance(-1), + _accel_class_instance(-1), + _gyro_reports(nullptr), + _gyro_scale{}, + _gyro_range_scale(0.0f), + _gyro_range_rad_s(0.0f), + _dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), + _sample_rate(1000), + _accel_reads(perf_alloc(PC_COUNT, "mpu9250_accel_read")), + _gyro_reads(perf_alloc(PC_COUNT, "mpu9250_gyro_read")), + _sample_perf(perf_alloc(PC_ELAPSED, "mpu9250_read")), + _bad_transfers(perf_alloc(PC_COUNT, "mpu9250_bad_transfers")), + _bad_registers(perf_alloc(PC_COUNT, "mpu9250_bad_registers")), + _good_transfers(perf_alloc(PC_COUNT, "mpu9250_good_transfers")), + _reset_retries(perf_alloc(PC_COUNT, "mpu9250_reset_retries")), + _duplicates(perf_alloc(PC_COUNT, "mpu9250_duplicates")), + _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), + _register_wait(0), + _reset_wait(0), + _accel_filter_x(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _rotation(rotation), + _checked_next(0), + _last_temperature(0), + _last_accel{}, + _got_duplicate(false) +{ + // disable debug() calls + _debug_enabled = false; + + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; + + /* Prime _gyro with parents devid. */ + _gyro->_device_id.devid = _device_id.devid; + _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU9250; + + // default accel scale factors + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + // default gyro scale factors + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + memset(&_call, 0, sizeof(_call)); +} + +MPU9250::~MPU9250() +{ + /* make sure we are truly inactive */ + stop(); + + /* delete the gyro subdriver */ + delete _gyro; + + /* free any existing reports */ + if (_accel_reports != nullptr) + delete _accel_reports; + if (_gyro_reports != nullptr) + delete _gyro_reports; + + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + + /* delete the perf counter */ + perf_free(_sample_perf); + perf_free(_accel_reads); + perf_free(_gyro_reads); + perf_free(_bad_transfers); + perf_free(_bad_registers); + perf_free(_good_transfers); + perf_free(_reset_retries); + perf_free(_duplicates); +} + +int +MPU9250::init() +{ + int ret; + + /* do SPI init (and probe) first */ + ret = SPI::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("SPI setup failed"); + return ret; + } + + /* allocate basic report buffers */ + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + if (_accel_reports == nullptr) + goto out; + + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + if (_gyro_reports == nullptr) + goto out; + + if (reset() != OK) + goto out; + + /* Initialize offsets and scales */ + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + + /* do CDev init for the gyro device node, keep it optional */ + ret = _gyro->init(); + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } + + _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + + measure(); + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + + if (_accel_topic == nullptr) { + warnx("ADVERT FAIL"); + } + + + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); + + _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + + if (_gyro->_gyro_topic == nullptr) { + warnx("ADVERT FAIL"); + } + +out: + return ret; +} + +int MPU9250::reset() +{ + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + up_udelay(10000); + + write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO); + up_udelay(1000); + + write_checked_reg(MPUREG_PWR_MGMT_2, 0); + up_udelay(1000); + + // SAMPLE RATE + _set_sample_rate(_sample_rate); + usleep(1000); + + // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) + // was 90 Hz, but this ruins quality and does not improve the + // system response + _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); + usleep(1000); + + // Gyro scale 2000 deg/s () + write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); + usleep(1000); + + // correct gyro scale factors + // scale to rad/s in SI units + // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s + // scaling factor: + // 1/(2^15)*(2000/180)*PI + _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); + _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; + + set_accel_range(16); + + usleep(1000); + + // INT CFG => Interrupt on Data Ready + write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready + usleep(1000); + write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read + usleep(1000); + + uint8_t retries = 10; + while (retries--) { + bool all_ok = true; + for (uint8_t i=0; i200) div=200; + if(div<1) div=1; + write_checked_reg(MPUREG_SMPLRT_DIV, div-1); + _sample_rate = 1000 / div; +} + +/* + set the DLPF filter frequency. This affects both accel and gyro. + */ +void +MPU9250::_set_dlpf_filter(uint16_t frequency_hz) +{ + uint8_t filter; + + /* + choose next highest filter frequency available + */ + if (frequency_hz == 0) { + _dlpf_freq = 0; + filter = BITS_DLPF_CFG_3600HZ; + } else if (frequency_hz <= 5) { + _dlpf_freq = 5; + filter = BITS_DLPF_CFG_5HZ; + } else if (frequency_hz <= 10) { + _dlpf_freq = 10; + filter = BITS_DLPF_CFG_10HZ; + } else if (frequency_hz <= 20) { + _dlpf_freq = 20; + filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 41) { + _dlpf_freq = 41; + filter = BITS_DLPF_CFG_41HZ; + } else if (frequency_hz <= 92) { + _dlpf_freq = 92; + filter = BITS_DLPF_CFG_92HZ; + } else if (frequency_hz <= 184) { + _dlpf_freq = 184; + filter = BITS_DLPF_CFG_184HZ; + } else if (frequency_hz <= 250) { + _dlpf_freq = 250; + filter = BITS_DLPF_CFG_250HZ; + } else { + _dlpf_freq = 0; + filter = BITS_DLPF_CFG_3600HZ; + } + write_checked_reg(MPUREG_CONFIG, filter); +} + +ssize_t +MPU9250::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(accel_report); + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _accel_reports->flush(); + measure(); + } + + /* if no data, error (we could block here) */ + if (_accel_reports->empty()) + return -EAGAIN; + + perf_count(_accel_reads); + + /* copy reports out of our buffer to the caller */ + accel_report *arp = reinterpret_cast(buffer); + int transferred = 0; + while (count--) { + if (!_accel_reports->get(arp)) + break; + transferred++; + arp++; + } + + /* return the number of bytes transferred */ + return (transferred * sizeof(accel_report)); +} + +int +MPU9250::self_test() +{ + if (perf_event_count(_sample_perf) == 0) { + measure(); + } + + /* return 0 on success, 1 else */ + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; +} + +int +MPU9250::accel_self_test() +{ + if (self_test()) + return 1; + + /* inspect accel offsets */ + if (fabsf(_accel_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + return 1; + + return 0; +} + +int +MPU9250::gyro_self_test() +{ + if (self_test()) + return 1; + + /* + * Maximum deviation of 20 degrees, according to + * http://www.invensense.com/mems/gyro/documents/PS-MPU-9250A-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; + + /* 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) > max_offset) + return 1; + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + return 1; + + if (fabsf(_gyro_scale.z_offset) > max_offset) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + return 1; + + /* check if all scales are zero */ + if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + /* if all are zero, this device is not calibrated */ + return 1; + } + + return 0; +} + + + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +MPU9250::test_error() +{ + // deliberately trigger an error. This was noticed during + // development as a handy way to test the reset logic + uint8_t data[16]; + memset(data, 0, sizeof(data)); + transfer(data, data, sizeof(data)); + ::printf("error triggered\n"); + print_registers(); +} + +ssize_t +MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(gyro_report); + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _gyro_reports->flush(); + measure(); + } + + /* if no data, error (we could block here) */ + if (_gyro_reports->empty()) + return -EAGAIN; + + perf_count(_gyro_reads); + + /* copy reports out of our buffer to the caller */ + gyro_report *grp = reinterpret_cast(buffer); + int transferred = 0; + while (count--) { + if (!_gyro_reports->get(grp)) + break; + transferred++; + grp++; + } + + /* return the number of bytes transferred */ + return (transferred * sizeof(gyro_report)); +} + +int +MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCRESET: + return reset(); + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + return ioctl(filp, SENSORIOCSPOLLRATE, 1000); + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + // adjust filters + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + _set_dlpf_filter(cutoff_freq_hz); + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + + + float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); + _set_dlpf_filter(cutoff_freq_hz_gyro); + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call_interval = ticks; + + /* + set call interval faster then the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu9250 clock + */ + _call.period = _call_interval - MPU9250_TIMER_REDUCTION; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _accel_reports->size(); + + case ACCELIOCGSAMPLERATE: + return _sample_rate; + + case ACCELIOCSSAMPLERATE: + _set_sample_rate(arg); + return OK; + + case ACCELIOCGLOWPASS: + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSLOWPASS: + // set software filtering + _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + return OK; + + case ACCELIOCSSCALE: + { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; + } + } + + case ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; + + case ACCELIOCSRANGE: + return set_accel_range(arg); + + case ACCELIOCGRANGE: + return (unsigned long)((_accel_range_m_s2)/MPU9250_ONE_G + 0.5f); + + case ACCELIOCSELFTEST: + return accel_self_test(); + + case ACCELIOCSHWLOWPASS: + _set_dlpf_filter(arg); + return OK; + + case ACCELIOCGHWLOWPASS: + return _dlpf_freq; + + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +int +MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + /* these are shared with the accel side */ + case SENSORIOCSPOLLRATE: + case SENSORIOCGPOLLRATE: + case SENSORIOCRESET: + return ioctl(filp, cmd, arg); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _gyro_reports->size(); + + case GYROIOCGSAMPLERATE: + return _sample_rate; + + case GYROIOCSSAMPLERATE: + _set_sample_rate(arg); + return OK; + + case GYROIOCGLOWPASS: + return _gyro_filter_x.get_cutoff_freq(); + + case GYROIOCSLOWPASS: + // set software filtering + _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + return OK; + + case GYROIOCSSCALE: + /* copy scale in */ + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); + return OK; + + case GYROIOCGSCALE: + /* copy scale out */ + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); + return OK; + + case GYROIOCSRANGE: + /* XXX not implemented */ + // XXX change these two values on set: + // _gyro_range_scale = xx + // _gyro_range_rad_s = xx + return -EINVAL; + case GYROIOCGRANGE: + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); + + case GYROIOCSELFTEST: + return gyro_self_test(); + + case GYROIOCSHWLOWPASS: + _set_dlpf_filter(arg); + return OK; + + case GYROIOCGHWLOWPASS: + return _dlpf_freq; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +MPU9250::read_reg(unsigned reg, uint32_t speed) +{ + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; + + // general register transfer at low clock speed + set_frequency(speed); + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +uint16_t +MPU9250::read_reg16(unsigned reg) +{ + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; + + // general register transfer at low clock speed + set_frequency(MPU9250_LOW_BUS_SPEED); + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} + +void +MPU9250::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + // general register transfer at low clock speed + set_frequency(MPU9250_LOW_BUS_SPEED); + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +MPU9250::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +void +MPU9250::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; i 8) { // 16g - AFS_SEL = 3 + afs_sel = 3; + lsb_per_g = 2048; + max_accel_g = 16; + } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 + afs_sel = 2; + lsb_per_g = 4096; + max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 + afs_sel = 1; + lsb_per_g = 8192; + max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 + afs_sel = 0; + lsb_per_g = 16384; + max_accel_g = 2; + } + + write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); + _accel_range_scale = (MPU9250_ONE_G / lsb_per_g); + _accel_range_m_s2 = max_accel_g * MPU9250_ONE_G; + + return OK; +} + +void +MPU9250::start() +{ + /* make sure we are stopped first */ + stop(); + + /* discard any stale data in the buffers */ + _accel_reports->flush(); + _gyro_reports->flush(); + + /* start polling at the specified rate */ + hrt_call_every(&_call, + 1000, + _call_interval-MPU9250_TIMER_REDUCTION, + (hrt_callout)&MPU9250::measure_trampoline, this); +} + +void +MPU9250::stop() +{ + hrt_cancel(&_call); +} + +void +MPU9250::measure_trampoline(void *arg) +{ + MPU9250 *dev = reinterpret_cast(arg); + + /* make another measurement */ + dev->measure(); +} + +void +MPU9250::check_registers(void) +{ + /* + we read the register at full speed, even though it isn't + listed as a high speed register. The low speed requirement + for some registers seems to be a propgation delay + requirement for changing sensor configuration, which should + not apply to reading a single register. It is also a better + test of SPI bus health to read at the same speed as we read + the data registers. + */ + uint8_t v; + if ((v=read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != + _checked_values[_checked_next]) { + _checked_bad[_checked_next] = v; + + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. + */ + if (_register_wait == 0 || _checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + write_reg(MPUREG_PWR_MGMT_2, MPU_CLK_SEL_AUTO); + // after doing a reset we need to wait a long + // time before we do any other register writes + // or we will end up with the mpu9250 in a + // bizarre state where it has all correct + // register values but large offsets on the + // accel axes + _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; + } else { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; + } + _register_wait = 20; + } + _checked_next = (_checked_next+1) % MPU9250_NUM_CHECKED_REGISTERS; +} + +void +MPU9250::measure() +{ + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + + struct MPUReport mpu_report; + struct Report { + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + int16_t temp; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + } report; + + /* start measuring */ + perf_begin(_sample_perf); + + /* + * Fetch the full set of measurements from the MPU9250 in one pass. + */ + mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + + // sensor transfer at high clock speed + set_frequency(MPU9250_HIGH_BUS_SPEED); + + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) + return; + + check_registers(); + + /* + see if this is duplicate accelerometer data. Note that we + can't use the data ready interrupt status bit in the status + register as that also goes high on new gyro data, and when + we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being + sampled at 8kHz, so we would incorrectly think we have new + data when we are in fact getting duplicate accelerometer data. + */ + if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { + // it isn't new data - wait for next timer + perf_end(_sample_perf); + perf_count(_duplicates); + _got_duplicate = true; + return; + } + memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); + _got_duplicate = false; + + /* + * Convert from big to little endian + */ + + report.accel_x = int16_t_from_bytes(mpu_report.accel_x); + report.accel_y = int16_t_from_bytes(mpu_report.accel_y); + report.accel_z = int16_t_from_bytes(mpu_report.accel_z); + + report.temp = int16_t_from_bytes(mpu_report.temp); + + report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); + report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); + report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); + + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0 && + report.temp == 0 && + report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, + return; + } + + perf_count(_good_transfers); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. We still increment + // _good_transfers, but don't return any data yet + _register_wait--; + return; + } + + + /* + * Swap axes and negate y + */ + int16_t accel_xt = report.accel_y; + int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); + + int16_t gyro_xt = report.gyro_y; + int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); + + /* + * Apply the swap + */ + report.accel_x = accel_xt; + report.accel_y = accel_yt; + report.gyro_x = gyro_xt; + report.gyro_y = gyro_yt; + + /* + * Report buffers. + */ + accel_report arb; + gyro_report grb; + + /* + * Adjust and scale results to m/s^2. + */ + grb.timestamp = arb.timestamp = hrt_absolute_time(); + + // report the error count as the sum of the number of bad + // transfers and bad register reads. This allows the higher + // level code to decide if it should use this sensor based on + // whether it has had failures + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + /* NOTE: Axes have been swapped to match the board a few lines above. */ + + arb.x_raw = report.accel_x; + arb.y_raw = report.accel_y; + arb.z_raw = report.accel_z; + + float xraw_f = report.accel_x; + float yraw_f = report.accel_y; + float zraw_f = report.accel_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + arb.x = _accel_filter_x.apply(x_in_new); + arb.y = _accel_filter_y.apply(y_in_new); + arb.z = _accel_filter_z.apply(z_in_new); + + arb.scaling = _accel_range_scale; + arb.range_m_s2 = _accel_range_m_s2; + + _last_temperature = (report.temp) / 361.0f + 35.0f; + + arb.temperature_raw = report.temp; + arb.temperature = _last_temperature; + + grb.x_raw = report.gyro_x; + grb.y_raw = report.gyro_y; + grb.z_raw = report.gyro_z; + + xraw_f = report.gyro_x; + yraw_f = report.gyro_y; + zraw_f = report.gyro_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + + grb.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.z = _gyro_filter_z.apply(z_gyro_in_new); + + grb.scaling = _gyro_range_scale; + grb.range_rad_s = _gyro_range_rad_s; + + grb.temperature_raw = report.temp; + grb.temperature = _last_temperature; + + _accel_reports->force(&arb); + _gyro_reports->force(&grb); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + _gyro->parent_poll_notify(); + + if (!(_pub_blocked)) { + /* log the time of this report */ + perf_begin(_controller_latency_perf); + perf_begin(_system_latency_perf); + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } + + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + } + + /* stop measuring */ + perf_end(_sample_perf); +} + +void +MPU9250::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_accel_reads); + perf_print_counter(_gyro_reads); + perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); + perf_print_counter(_good_transfers); + perf_print_counter(_reset_retries); + perf_print_counter(_duplicates); + _accel_reports->print_info("accel queue"); + _gyro_reports->print_info("gyro queue"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; igyro_read(filp, buffer, buflen); +} + +int +MPU9250_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->gyro_ioctl(filp, cmd, arg); + } +} + +/** + * Local functions in support of the shell command. + */ +namespace mpu9250 +{ + +MPU9250 *g_dev_int; // on internal bus +MPU9250 *g_dev_ext; // on external bus + +void start(bool, enum Rotation); +void stop(bool); +void test(bool); +void reset(bool); +void info(bool); +void regdump(bool); +void testerror(bool); +void usage(); + +/** + * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. + */ +void +start(bool external_bus, enum Rotation rotation) +{ + int fd; + MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + + if (*g_dev_ptr != nullptr) + /* if already started, the still command succeeded */ + errx(0, "already started"); + + /* create the driver */ + if (external_bus) { +#ifdef PX4_SPI_BUS_EXT + *g_dev_ptr = new MPU9250(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); +#else + errx(0, "External SPI not available"); +#endif + } else { + *g_dev_ptr = new MPU9250(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); + } + + if (*g_dev_ptr == nullptr) + goto fail; + + if (OK != (*g_dev_ptr)->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(path_accel, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + close(fd); + + exit(0); +fail: + + if (*g_dev_ptr != nullptr) { + delete (*g_dev_ptr); + *g_dev_ptr = nullptr; + } + + errx(1, "driver start failed"); +} + +void +stop(bool external_bus) +{ + MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr != nullptr) { + delete *g_dev_ptr; + *g_dev_ptr = nullptr; + } else { + /* warn, but not an error */ + warnx("already stopped."); + } + 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(bool external_bus) +{ + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + accel_report a_report; + gyro_report g_report; + ssize_t sz; + + /* get the driver */ + int fd = open(path_accel, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'mpu9250 start')", + path_accel); + + /* get the driver */ + int fd_gyro = open(path_gyro, O_RDONLY); + + if (fd_gyro < 0) + err(1, "%s open failed", path_gyro); + + /* reset to manual polling */ + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + err(1, "reset to manual polling"); + + /* do a simple demand read */ + sz = read(fd, &a_report, sizeof(a_report)); + + if (sz != sizeof(a_report)) { + warnx("ret: %d, expected: %d", sz, sizeof(a_report)); + err(1, "immediate acc read failed"); + } + + warnx("single read"); + warnx("time: %lld", a_report.timestamp); + warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x); + warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y); + warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z); + warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); + warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); + warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); + warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, + (double)(a_report.range_m_s2 / MPU9250_ONE_G)); + + /* do a simple demand read */ + sz = read(fd_gyro, &g_report, sizeof(g_report)); + + if (sz != sizeof(g_report)) { + warnx("ret: %d, expected: %d", sz, sizeof(g_report)); + err(1, "immediate gyro read failed"); + } + + warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); + warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); + warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("gyro x: \t%d\traw", (int)g_report.x_raw); + warnx("gyro y: \t%d\traw", (int)g_report.y_raw); + warnx("gyro z: \t%d\traw", (int)g_report.z_raw); + warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + + warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); + warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); + + + /* XXX add poll-rate tests here too */ + + reset(external_bus); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset(bool external_bus) +{ + const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + int fd = open(path_accel, 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"); + + close(fd); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info(bool external_bus) +{ + MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_info(); + + exit(0); +} + +/** + * Dump the register information + */ +void +regdump(bool external_bus) +{ + MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + printf("regdump @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_registers(); + + exit(0); +} + +/** + * deliberately produce an error to test recovery + */ +void +testerror(bool external_bus) +{ + MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + (*g_dev_ptr)->test_error(); + + exit(0); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'"); + warnx("options:"); + warnx(" -X (external bus)"); + warnx(" -R rotation"); +} + +} // namespace + +int +mpu9250_main(int argc, char *argv[]) +{ + bool external_bus = false; + int ch; + enum Rotation rotation = ROTATION_NONE; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "XR:")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; + default: + mpu9250::usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + + /* + * Start/load the driver. + + */ + if (!strcmp(verb, "start")) { + mpu9250::start(external_bus, rotation); + } + + if (!strcmp(verb, "stop")) { + mpu9250::stop(external_bus); + } + + /* + * Test the driver/device. + */ + if (!strcmp(verb, "test")) { + mpu9250::test(external_bus); + } + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) { + mpu9250::reset(external_bus); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + mpu9250::info(external_bus); + } + + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) { + mpu9250::regdump(external_bus); + } + + if (!strcmp(verb, "testerror")) { + mpu9250::testerror(external_bus); + } + + mpu9250::usage(); + exit(1); +} From 1b15be6fd861c041cc47a3c2aa70e73808c6795c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 1 Aug 2015 16:33:13 +1000 Subject: [PATCH 160/293] drv_sensor: added MPU9250 sensor defines --- src/drivers/drv_sensor.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index a98bcafc7aa8..a402f327b774 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -60,9 +60,11 @@ #define DRV_ACC_DEVTYPE_MPU6000 0x13 #define DRV_ACC_DEVTYPE_ACCELSIM 0x14 #define DRV_ACC_DEVTYPE_GYROSIM 0x15 +#define DRV_ACC_DEVTYPE_MPU9250 0x16 #define DRV_GYR_DEVTYPE_MPU6000 0x21 #define DRV_GYR_DEVTYPE_L3GD20 0x22 #define DRV_GYR_DEVTYPE_GYROSIM 0x23 +#define DRV_GYR_DEVTYPE_MPU9250 0x24 #define DRV_RNG_DEVTYPE_MB12XX 0x31 #define DRV_RNG_DEVTYPE_LL40LS 0x32 From ec4127588fec73814ab2760f01968339e6701d54 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Aug 2015 16:13:55 +1000 Subject: [PATCH 161/293] mpu6000: check WHOAMI register this prevents a mpu9250 being seen as a mpu6000 --- src/drivers/mpu6000/mpu6000.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 92cfa28010a4..1b300194b6a8 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -147,6 +147,8 @@ #define BIT_I2C_IF_DIS 0x10 #define BIT_INT_STATUS_DATA 0x01 +#define MPU_WHOAMI_6000 0x68 + // Product ID Description for MPU6000 // high 4 bits low 4 bits // Product Name Product Revision @@ -758,6 +760,13 @@ int MPU6000::reset() int MPU6000::probe() { + uint8_t whoami; + whoami = read_reg(MPUREG_WHOAMI); + if (whoami != MPU_WHOAMI_6000) { + debug("unexpected WHOAMI 0x%02x", whoami); + return -EIO; + + } /* look for a product ID we recognise */ _product = read_reg(MPUREG_PRODUCT_ID); From 8470351f76b7c268c084a31f323a2781873c360e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Aug 2015 12:01:13 +1000 Subject: [PATCH 162/293] px4fmu: allow for GPIO_SET_OUTPUT with initial value this is needed to prevent inadvertent camera trigger when setting up a port --- src/drivers/drv_gpio.h | 6 ++++++ src/drivers/px4fmu/fmu.cpp | 14 +++++++++++++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index a0faab809844..c91724036c28 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -170,4 +170,10 @@ #define GPIO_PERIPHERAL_RAIL_RESET GPIOC(14) +/** configure the board GPIOs in (arg) as outputs, initially low */ +#define GPIO_SET_OUTPUT_LOW GPIOC(15) + +/** configure the board GPIOs in (arg) as outputs, initially high */ +#define GPIO_SET_OUTPUT_HIGH GPIOC(16) + #endif /* _DRV_GPIO_H */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bf0d5e8d6f54..b7538d270de0 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1474,7 +1474,9 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) gpios |= 3; /* flip the buffer to output mode if required */ - if (GPIO_SET_OUTPUT == function) + if (GPIO_SET_OUTPUT == function || + GPIO_SET_OUTPUT_LOW == function || + GPIO_SET_OUTPUT_HIGH == function) stm32_gpiowrite(GPIO_GPIO_DIR, 1); } @@ -1492,6 +1494,14 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) stm32_configgpio(_gpio_tab[i].output); break; + case GPIO_SET_OUTPUT_LOW: + stm32_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_SET)) | GPIO_OUTPUT_CLEAR); + break; + + case GPIO_SET_OUTPUT_HIGH: + stm32_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_CLEAR)) | GPIO_OUTPUT_SET); + break; + case GPIO_SET_ALT_1: if (_gpio_tab[i].alt != 0) stm32_configgpio(_gpio_tab[i].alt); @@ -1554,6 +1564,8 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) break; case GPIO_SET_OUTPUT: + case GPIO_SET_OUTPUT_LOW: + case GPIO_SET_OUTPUT_HIGH: case GPIO_SET_INPUT: case GPIO_SET_ALT_1: gpio_set_function(arg, cmd); From 20eb125de66e279901fe89253c4f2c6c1d656b28 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Aug 2015 18:01:09 +1000 Subject: [PATCH 163/293] pwm_input: removed Lidar specific reset code the PWM input driver is not the right place to have Lidar specific code. It is intended to be a generic PWM input driver, and it should not be touching other pins. Doing so prevents those pins from being used for other purposes --- src/drivers/pwm_input/pwm_input.cpp | 46 ----------------------------- 1 file changed, 46 deletions(-) diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 800bc85ba093..74d652db9416 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -86,9 +86,6 @@ #include #include -/* Reset pin define */ -#define GPIO_VDD_RANGEFINDER_EN GPIO_GPIO5_OUTPUT - #if HRT_TIMER == PWMIN_TIMER #error cannot share timer between HRT and PWMIN #endif @@ -241,7 +238,6 @@ class PWMIN : device::CDev void publish(uint16_t status, uint32_t period, uint32_t pulse_width); void print_info(void); - void hard_reset(); private: uint32_t _error_count; @@ -253,19 +249,11 @@ class PWMIN : device::CDev ringbuffer::RingBuffer *_reports; bool _timer_started; - hrt_call _hard_reset_call; /* HRT callout for note completion */ - hrt_call _freeze_test_call; /* HRT callout for note completion */ - perf_counter_t _perf_reset; perf_counter_t _perf_interrupt; perf_counter_t _perf_read; void _timer_init(void); - - void _turn_on(); - void _turn_off(); - void _freeze_test(); - }; static int pwmin_tim_isr(int irq, void *context); @@ -318,9 +306,6 @@ PWMIN::init() return -ENOMEM; } - /* Schedule freeze check to invoke periodically */ - hrt_call_every(&_freeze_test_call, 0, TIMEOUT_POLL, reinterpret_cast(&PWMIN::_freeze_test), this); - return OK; } @@ -384,34 +369,6 @@ void PWMIN::_timer_init(void) perf_count(_perf_reset); } -void -PWMIN::_freeze_test() -{ - /* reset if last poll time was way back and a read was recently requested */ - if (hrt_elapsed_time(&_last_poll_time) > TIMEOUT_POLL && hrt_elapsed_time(&_last_read_time) < TIMEOUT_READ) { - hard_reset(); - } -} - -void -PWMIN::_turn_on() -{ - stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1); -} - -void -PWMIN::_turn_off() -{ - stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0); -} - -void -PWMIN::hard_reset() -{ - _turn_off(); - hrt_call_after(&_hard_reset_call, 9000, reinterpret_cast(&PWMIN::_turn_on), this); -} - /* * hook for open of the driver. We start the timer at this point, then * leave it running @@ -466,8 +423,6 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) * be needed if the pin was used for a different * purpose (such as PWM output) */ _timer_init(); - /* also reset the sensor */ - hard_reset(); return OK; default: @@ -630,7 +585,6 @@ static void pwmin_test(void) */ static void pwmin_reset(void) { - g_dev->hard_reset(); int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); if (fd == -1) { From df0d8f0a4a7bff4d60759ce5bcb532391fe242e5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Aug 2015 13:33:03 +1000 Subject: [PATCH 164/293] ll40ls: new strategy for initialisating and operating the lidar on I2C on a V2 lidar we now use continuous mode, which avoids a bug where it sometimes gives bad values for the distance (apparently reading from the wrong register). On both lidars we change the reset strategy to wait until we get all of the required register values setup --- src/drivers/ll40ls/LidarLiteI2C.cpp | 133 ++++++++++++++++++++++++---- src/drivers/ll40ls/LidarLiteI2C.h | 23 +++-- 2 files changed, 133 insertions(+), 23 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index c6190aed50bc..964ad978ce28 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -74,6 +74,9 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : _pause_measurements(false), _hw_version(0), _sw_version(0), + _v2_hardware(false), + _reset_complete(false), + _same_value_counter(0), _bus(bus) { // up the retries since the device misses the first measure attempts @@ -201,8 +204,11 @@ int LidarLiteI2C::probe() return -EIO; ok: + // "blue label" V2 lidars have hw version 0x15 + _v2_hardware = (_hw_version >= 0x15); _retries = 3; - return reset_sensor(); + _reset_complete = false; + return OK; } int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg) @@ -302,6 +308,13 @@ ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen) int LidarLiteI2C::measure() { + if (_v2_hardware) { + // we use continuous mode for V2 hardware so there is + // no measure() phase. Experiments show that + // non-continuous mode is not reliable, often gives + // false values for distance + return OK; + } int ret; if (_pause_measurements) { @@ -311,11 +324,20 @@ int LidarLiteI2C::measure() return OK; } + /* + see if the reset of the sensor is complete. If it isn't + complete yet then don't do the measure() cycle this time. + */ + if (! _reset_complete) { + ret = reset_sensor(); + if (ret != OK) + return ret; + } + /* * Send the command to begin a measurement. */ - const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; - ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0); + ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE); if (OK != ret) { perf_count(_comms_errors); @@ -324,8 +346,7 @@ int LidarLiteI2C::measure() // if we are getting lots of I2C transfer errors try // resetting the sensor if (perf_event_count(_comms_errors) % 10 == 0) { - perf_count(_sensor_resets); - reset_sensor(); + _reset_complete = false; } return ret; @@ -340,17 +361,76 @@ int LidarLiteI2C::measure() } /* - reset the sensor to power on defaults + a table of settings for a lidar + */ +struct settings_table { + uint8_t reg; + uint8_t value; +}; + +/* + register setup table for V1 Lidar + */ +static const struct settings_table settings_v1[] = { + { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET }, +}; + +/* + register setup table for V2 Lidar + */ +static const struct settings_table settings_v2[] = { + { LL40LS_INTERVAL, 0x28 }, // 0x28 == 50Hz + { LL40LS_COUNT, LL40LS_COUNT_CONTINUOUS }, + { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }, +}; + +/* + reset the sensor to required settings. This is done by incrementally + setting all register values according to the hw version. It may take + multiple tries to get the sensor fully reset, as the sensor may not + respond immediately */ int LidarLiteI2C::reset_sensor() { - int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); - if (ret != OK) - return ret; + perf_count(_sensor_resets); + + // force _reset_complete here so when called via the ioctl it + // will keep trying the reset + _reset_complete = false; - // wait for sensor reset to complete - usleep(1000); + const struct settings_table *table; + uint8_t num_settings; + if (_v2_hardware) { + table = settings_v2; + num_settings = sizeof(settings_v2)/sizeof(settings_table); + } else { + table = settings_v1; + num_settings = sizeof(settings_v1)/sizeof(settings_table); + } + /* + we accept the sensor as reset when all registers have taken + on their expected values. + */ + for (uint8_t i=0; i= 200) { + /* + we are getting continuous identical values, + the sensor may need to be reset + */ + _reset_complete = false; + _same_value_counter = 0; + } + } else { + _same_value_counter = 0; + } _last_distance = distance_cm; diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index 51cca0862f5a..c0316a4e3191 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -60,14 +60,18 @@ /* LL40LS Registers addresses */ -#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ -#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */ -#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ -#define LL40LS_DISTHIGH_REG 0x0F /* High byte of distance register, auto increment */ -#define LL40LS_AUTO_INCREMENT 0x80 -#define LL40LS_HW_VERSION 0x41 -#define LL40LS_SW_VERSION 0x4f -#define LL40LS_SIGNAL_STRENGTH_REG 0x5b +#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ +#define LL40LS_DISTHIGH_REG 0x0F /* High byte of distance register, auto increment */ +#define LL40LS_COUNT 0x11 +#define LL40LS_HW_VERSION 0x41 +#define LL40LS_INTERVAL 0x45 +#define LL40LS_SW_VERSION 0x4f + +// bit values +#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */ +#define LL40LS_AUTO_INCREMENT 0x80 +#define LL40LS_COUNT_CONTINUOUS 0xff +#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ class LidarLiteI2C : public LidarLite, public device::I2C { @@ -119,6 +123,9 @@ class LidarLiteI2C : public LidarLite, public device::I2C volatile bool _pause_measurements; uint8_t _hw_version; uint8_t _sw_version; + bool _v2_hardware; + bool _reset_complete; + uint8_t _same_value_counter; /**< the bus the device is connected to */ int _bus; From 34e1d543e49c2756b8fa4b81ef30468497ea792a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Aug 2015 14:55:29 +1000 Subject: [PATCH 165/293] perf_counter: fixed write to correct fd for perf output this fixes perf output for nsh over MAVLink --- src/modules/systemlib/perf_counter.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 1344accb8497..b0a0b01d035e 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -377,7 +377,7 @@ perf_reset(perf_counter_t handle) void perf_print_counter(perf_counter_t handle) { - perf_print_counter_fd(0, handle); + perf_print_counter_fd(1, handle); } void From 51858c810a3cb086d7c2041328bd32adec9fa724 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 28 Aug 2015 20:49:50 +0900 Subject: [PATCH 166/293] IRLock: fix compile error --- src/drivers/irlock/irlock.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 4aeb8c7497c5..19b0de6d5fea 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -110,7 +110,7 @@ class IRLOCK : public device::I2C int read_device_block(struct irlock_s *block); /** internal variables **/ - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; work_s _work; uint32_t _read_failures; @@ -158,7 +158,7 @@ int IRLOCK::init() } /** allocate buffer storing values read from sensor **/ - _reports = new RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); + _reports = new ringbuffer::RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); if (_reports == nullptr) { return ENOTTY; From 1623d181b791ed0dfc151db66c2ed41ffb1b9219 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Sun, 13 Sep 2015 19:44:26 -0300 Subject: [PATCH 167/293] build: parallelize build While building PX4Firmware for ardupilot we see several messages like: make[1]: warning: jobserver unavailable: using -j1. Add '+' to parent make rule. make[1]: Entering directory '/home/lucas/p/dronecode/ardupilot' The problem is that we are recursing the makefiles by simply calling make and we don't let Make keep track of the jobs. There are some rules that need to be serialized, but they are not in PX4Firmware, it's Ardupilot that needs to serialize some of the recursive makes. --- Makefile | 24 ++++++++---------------- makefiles/firmware.mk | 8 ++++---- makefiles/firmware_nuttx.mk | 18 +++++------------- 3 files changed, 17 insertions(+), 33 deletions(-) diff --git a/Makefile b/Makefile index 00750228923a..359241eb0047 100644 --- a/Makefile +++ b/Makefile @@ -144,7 +144,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: generateuorbtopicheaders checksu @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% $(Q) $(MKDIR) -p $(work_dir) - $(Q) $(MAKE) -r -C $(work_dir) \ + $(Q)+ $(MAKE) -r -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ @@ -178,24 +178,16 @@ NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) .PHONY: archives archives: checksubmodules $(NUTTX_ARCHIVES) -# We cannot build these parallel; note that we also force -j1 for the -# sub-make invocations. -ifneq ($(filter archives,$(MAKECMDGOALS)),) -.NOTPARALLEL: -endif - -J?=1 - $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) $(ARCHIVE_DIR)%.export: configuration = nsh $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export $(Q) $(MKDIR) -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) @@ -212,11 +204,11 @@ BOARD = $(BOARDS) menuconfig: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(BOARD) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig @$(ECHO) %% Saving configuration file $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig else @@ -287,7 +279,7 @@ testbuild: nuttx posix qurt: ifeq ($(GOALS),) - make PX4_TARGET_OS=$@ $(GOALS) + +$(MAKE) PX4_TARGET_OS=$@ $(GOALS) else export PX4_TARGET_OS=$@ endif @@ -296,7 +288,7 @@ posixrun: Tools/posix_run.sh qurtrun: - make PX4_TARGET_OS=qurt sim + $(MAKE) PX4_TARGET_OS=qurt sim # # Unittest targets. Builds and runs the host-level @@ -325,7 +317,7 @@ clean: distclean: clean @echo > /dev/null $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export - $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) # diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index f5f5d8188d6a..d8d82e7b6c68 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -229,7 +229,7 @@ $(MODULE_OBJS): mkfile = $(patsubst %module.pre.o,%module.mk,$(relpath)) $(MODULE_OBJS): workdir = $(@D) $(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER) $(Q) $(MKDIR) -p $(workdir) - $(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \ + $(Q)+ $(MAKE) -r -f $(PX4_MK_DIR)module.mk \ -C $(workdir) \ MODULE_WORK_DIR=$(workdir) \ MODULE_OBJ=$@ \ @@ -246,7 +246,7 @@ $(MODULE_CLEANS): relpath = $(patsubst $(WORK_DIR)%,%,$@) $(MODULE_CLEANS): mkfile = $(patsubst %clean,%module.mk,$(relpath)) $(MODULE_CLEANS): @$(ECHO) %% cleaning using $(mkfile) - $(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \ + $(Q)+ $(MAKE) -r -f $(PX4_MK_DIR)module.mk \ MODULE_WORK_DIR=$(dir $@) \ MODULE_MK=$(mkfile) \ clean @@ -289,7 +289,7 @@ $(LIBRARY_LIBS): mkfile = $(patsubst %library.a,%library.mk,$(relpath)) $(LIBRARY_LIBS): workdir = $(@D) $(LIBRARY_LIBS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER) $(Q) $(MKDIR) -p $(workdir) - $(Q) $(MAKE) -r -f $(PX4_MK_DIR)library.mk \ + $(Q)+ $(MAKE) -r -f $(PX4_MK_DIR)library.mk \ -C $(workdir) \ LIBRARY_WORK_DIR=$(workdir) \ LIBRARY_LIB=$@ \ @@ -306,7 +306,7 @@ $(LIBRARY_CLEANS): relpath = $(patsubst $(WORK_DIR)%,%,$@) $(LIBRARY_CLEANS): mkfile = $(patsubst %clean,%library.mk,$(relpath)) $(LIBRARY_CLEANS): @$(ECHO) %% cleaning using $(mkfile) - $(Q) $(MAKE) -r -f $(PX4_MK_DIR)library.mk \ + $(Q)+ $(MAKE) -r -f $(PX4_MK_DIR)library.mk \ LIBRARY_WORK_DIR=$(dir $@) \ LIBRARY_MK=$(mkfile) \ clean diff --git a/makefiles/firmware_nuttx.mk b/makefiles/firmware_nuttx.mk index 77bb2bace0a3..5ab84b44cf44 100644 --- a/makefiles/firmware_nuttx.mk +++ b/makefiles/firmware_nuttx.mk @@ -62,7 +62,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: generateuorbtopicheaders checksu @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% $(Q) $(MKDIR) -p $(work_dir) - $(Q) $(MAKE) -r -C $(work_dir) \ + $(Q)+ $(MAKE) -r -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ @@ -96,24 +96,16 @@ NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) .PHONY: archives archives: checksubmodules $(NUTTX_ARCHIVES) -# We cannot build these parallel; note that we also force -j1 for the -# sub-make invocations. -ifneq ($(filter archives,$(MAKECMDGOALS)),) -.NOTPARALLEL: -endif - -J?=1 - $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) $(ARCHIVE_DIR)%.export: configuration = nsh $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export $(Q) $(MKDIR) -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) @@ -130,11 +122,11 @@ BOARD = $(BOARDS) menuconfig: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(BOARD) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig @$(ECHO) %% Saving configuration file $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig else From c4f390286e7821c5839688cb6d1d1a11972332ad Mon Sep 17 00:00:00 2001 From: Michael Day Date: Fri, 4 Sep 2015 12:47:54 -0700 Subject: [PATCH 168/293] sdlog2: Added 'd' type to FORMAT_TO_STRUCT. Fixes an exception we had been having when using sdlog2_dump.py with dataflash logs: Exception: Unsupported format char: d in message GRAW (171) --- Tools/sdlog2/sdlog2_dump.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/sdlog2/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py index 8b0ccb2d7de3..aeb60139c5e9 100755 --- a/Tools/sdlog2/sdlog2_dump.py +++ b/Tools/sdlog2/sdlog2_dump.py @@ -46,6 +46,7 @@ class SDLog2Parser: "h": ("h", None), "H": ("H", None), "i": ("i", None), + "d": ("d", None), "I": ("I", None), "f": ("f", None), "n": ("4s", None), From c340616517c4736016898077bb9ac3ed3818a275 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Sep 2015 09:03:13 +1000 Subject: [PATCH 169/293] ms5611: reduced OSR to 1024 this reduces self-heating of the sensor which reduces the amount of altitude change when warming up. Apparently some individual sensors are severely affected by this. Unfortunately it raises the noise level, but Paul is confident it won't be a significant issue --- src/drivers/ms5611/ms5611.h | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index 1d6239467d32..8bb8f3640c13 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -38,8 +38,26 @@ */ #define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ -#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */ -#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D1_OSR256 0x40 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR512 0x42 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR1024 0x44 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR2048 0x46 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR4096 0x48 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D2_OSR256 0x50 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR512 0x52 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR1024 0x54 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR2048 0x56 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR4096 0x58 /* write to this address to start temperature conversion */ + +/* + use an OSR of 1024 to reduce the self-heating effect of the + sensor. Information from MS tells us that some individual sensors + are quite sensitive to this effect and that reducing the OSR can + make a big difference + */ +#define ADDR_CMD_CONVERT_D1 ADDR_CMD_CONVERT_D1_OSR1024 +#define ADDR_CMD_CONVERT_D2 ADDR_CMD_CONVERT_D2_OSR1024 + #define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ #define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ #define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ From a1797dc1a71151239dd807d0c36719729bce5cc5 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Wed, 18 Nov 2015 10:27:54 +1100 Subject: [PATCH 170/293] make: support new device id pci-3D_Robotics after Ubuntu 15.10 upgrade --- makefiles/upload.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/upload.mk b/makefiles/upload.mk index c590f17d102b..e73c31dc313c 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -15,7 +15,7 @@ ifeq ($(SYSTYPE),Darwin) SERIAL_PORTS ?= "/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" endif ifeq ($(SYSTYPE),Linux) -SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*" +SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/pci-3D_Robotics*" endif ifeq ($(SERIAL_PORTS),) 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" From 1f9983a207bafbddfd30bde341977a102665636e Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 08:41:16 -1000 Subject: [PATCH 171/293] Support PX4IO_DEVICE_PATH not defined --- src/modules/gpio_led/gpio_led.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 2ff3fc27677e..f8c62a88a73f 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -89,7 +89,7 @@ int gpio_led_main(int argc, char *argv[]) "\t\tr2\tPX4IO RELAY2" ); #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) errx(1, "usage: gpio_led {start|stop} [-p ]\n" "\t-p \tUse specified AUX OUT pin number (default: 1)" ); @@ -111,7 +111,7 @@ int gpio_led_main(int argc, char *argv[]) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 char *pin_name = "PX4FMU GPIO_EXT1"; #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) char pin_name[] = "AUX OUT 1"; #endif @@ -154,7 +154,7 @@ int gpio_led_main(int argc, char *argv[]) } #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) unsigned int n = strtoul(argv[3], NULL, 10); if (n >= 1 && n <= 6) { @@ -207,12 +207,16 @@ void gpio_led_start(FAR void *arg) char *gpio_dev; +#if defined(PX4IO_DEVICE_PATH) if (priv->use_io) { gpio_dev = PX4IO_DEVICE_PATH; } else { gpio_dev = PX4FMU_DEVICE_PATH; } +#else + gpio_dev = PX4FMU_DEVICE_PATH; +#endif /* open GPIO device */ priv->gpio_fd = open(gpio_dev, 0); From 9e0dbaf896e6a7689435d94e818def2d402fd019 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 08:52:36 -1000 Subject: [PATCH 172/293] Support GPIO_CAN2_RX not defined --- src/modules/uavcan/uavcan_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5b22d4b3c817..c11693cca392 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -150,10 +150,17 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to * fail during initialization. */ +#if defined(GPIO_CAN1_RX) stm32_configgpio(GPIO_CAN1_RX); stm32_configgpio(GPIO_CAN1_TX); +#endif +#if defined(GPIO_CAN2_RX) stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); stm32_configgpio(GPIO_CAN2_TX); +#endif +#if !defined(GPIO_CAN1_RX) && !defined(GPIO_CAN2_RX) +# error "Need to define GPIO_CAN1_RX and/or GPIO_CAN2_RX" +#endif /* * CAN driver init From ee8cb7cc5a7319bdbe53657ee63eef64fe49d97c Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 14:39:32 -1000 Subject: [PATCH 173/293] Added Fix me re chan 1/2 interactions --- src/drivers/stm32/drv_hrt.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index e37b750fe88f..17a69b03377e 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -221,6 +221,7 @@ /* * Specific registers and bits used by HRT sub-functions */ +/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/ #if HRT_TIMER_CHANNEL == 1 # define rCCR_HRT rCCR1 /* compare register for HRT */ # define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */ @@ -294,7 +295,7 @@ static void hrt_call_invoke(void); # define GTIM_CCER_CC4NP 0 # define PPM_EDGE_FLIP # endif - +/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/ # if HRT_PPM_CHANNEL == 1 # define rCCR_PPM rCCR1 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */ From 8ba5447b24294f51ed2dcfb7122cd3fa234a894c Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 13 Nov 2015 12:39:00 -1000 Subject: [PATCH 174/293] Wip FMUV3 --- Images/px4fmu-v3.prototype | 12 + cmake/configs/nuttx_px4fmu-v3_default.cmake | 178 +++ nuttx-configs/px4fmu-v3/include/board.h | 295 +++++ .../px4fmu-v3/include/nsh_romfsimg.h | 42 + nuttx-configs/px4fmu-v3/nsh/Make.defs | 177 +++ nuttx-configs/px4fmu-v3/nsh/appconfig | 52 + nuttx-configs/px4fmu-v3/nsh/defconfig | 1068 +++++++++++++++++ nuttx-configs/px4fmu-v3/nsh/setenv.sh | 67 ++ nuttx-configs/px4fmu-v3/scripts/ld.script | 159 +++ nuttx-configs/px4fmu-v3/src/Makefile | 84 ++ nuttx-configs/px4fmu-v3/src/empty.c | 4 + src/drivers/boards/px4fmu-v3/CMakeLists.txt | 47 + src/drivers/boards/px4fmu-v3/board_config.h | 279 +++++ src/drivers/boards/px4fmu-v3/px4fmu3_init.c | 344 ++++++ src/drivers/boards/px4fmu-v3/px4fmu3_led.c | 106 ++ src/drivers/boards/px4fmu-v3/px4fmu_can.c | 144 +++ .../boards/px4fmu-v3/px4fmu_pwm_servo.c | 105 ++ src/drivers/boards/px4fmu-v3/px4fmu_spi.c | 163 +++ src/drivers/boards/px4fmu-v3/px4fmu_usb.c | 108 ++ src/drivers/drv_gpio.h | 25 +- src/drivers/drv_led.h | 2 + src/drivers/meas_airspeed/meas_airspeed.cpp | 4 +- src/drivers/px4fmu/fmu.cpp | 13 +- src/drivers/stm32/adc/adc.cpp | 34 + src/lib/version/version.h | 4 + 25 files changed, 3512 insertions(+), 4 deletions(-) create mode 100644 Images/px4fmu-v3.prototype create mode 100644 cmake/configs/nuttx_px4fmu-v3_default.cmake create mode 100755 nuttx-configs/px4fmu-v3/include/board.h create mode 100644 nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h create mode 100644 nuttx-configs/px4fmu-v3/nsh/Make.defs create mode 100644 nuttx-configs/px4fmu-v3/nsh/appconfig create mode 100644 nuttx-configs/px4fmu-v3/nsh/defconfig create mode 100755 nuttx-configs/px4fmu-v3/nsh/setenv.sh create mode 100644 nuttx-configs/px4fmu-v3/scripts/ld.script create mode 100644 nuttx-configs/px4fmu-v3/src/Makefile create mode 100644 nuttx-configs/px4fmu-v3/src/empty.c create mode 100644 src/drivers/boards/px4fmu-v3/CMakeLists.txt create mode 100644 src/drivers/boards/px4fmu-v3/board_config.h create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu3_init.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu3_led.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_can.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_spi.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_usb.c diff --git a/Images/px4fmu-v3.prototype b/Images/px4fmu-v3.prototype new file mode 100644 index 000000000000..ada86b9a93ff --- /dev/null +++ b/Images/px4fmu-v3.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 11, + "magic": "PX4FWv1", + "description": "Firmware for the PX4FMUv3 board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv3", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake new file mode 100644 index 000000000000..82f4c1b9a799 --- /dev/null +++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake @@ -0,0 +1,178 @@ +include(nuttx/px4_impl_nuttx) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/stm32 + drivers/stm32/adc + drivers/stm32/tone_alarm + drivers/led + drivers/px4fmu + drivers/boards/px4fmu-v3 + drivers/rgbled + drivers/mpu9250 + drivers/hmc5883 + drivers/ms5611 + drivers/mb12xx + drivers/srf02 + drivers/sf0x + drivers/ll40ls + drivers/trone + drivers/gps + drivers/pwm_out_sim + drivers/hott + drivers/hott/hott_telemetry + drivers/hott/hott_sensors + drivers/blinkm + drivers/airspeed + drivers/ets_airspeed + drivers/meas_airspeed + drivers/frsky_telemetry + modules/sensors + #drivers/mkblctrl + drivers/px4flow + drivers/oreoled + drivers/gimbal + drivers/pwm_input + drivers/camera_trigger + + # + # System commands + # + systemcmds/bl_update + systemcmds/mixer + systemcmds/param + systemcmds/perf + systemcmds/pwm + systemcmds/esc_calib + systemcmds/reboot + #systemcmds/topic_listener + systemcmds/top + systemcmds/config + systemcmds/nshterm + systemcmds/mtd + systemcmds/dumpfile + systemcmds/ver + + # + # General system control + # + modules/commander + modules/navigator + modules/mavlink + modules/gpio_led + modules/uavcan + modules/land_detector + + # + # Estimation modules (EKF/ SO3 / other filters) + # + # Too high RAM usage due to static allocations + # modules/attitude_estimator_ekf + modules/attitude_estimator_q + modules/ekf_att_pos_estimator + modules/position_estimator_inav + + # + # Vehicle Control + # + # modules/segway # XXX Needs GCC 4.7 fix + modules/fw_pos_control_l1 + modules/fw_att_control + modules/mc_att_control + modules/mc_pos_control + modules/vtol_att_control + + # + # Logging + # + modules/sdlog2 + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/controllib + modules/uORB + modules/dataman + + # + # Libraries + # + #lib/mathlib/CMSIS + lib/mathlib + lib/mathlib/math/filter + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/conversion + lib/launchdetection + platforms/nuttx + + # had to add for cmake, not sure why wasn't in original config + platforms/common + platforms/nuttx/px4_layer + + # + # OBC challenge + # + modules/bottle_drop + + # + # Rover apps + # + examples/rover_steering_control + + # + # Demo apps + # + #examples/math_demo + # Tutorial code from + # https://px4.io/dev/px4_simple_app + #examples/px4_simple_app + + # Tutorial code from + # https://px4.io/dev/daemon + #examples/px4_daemon_app + + # Tutorial code from + # https://px4.io/dev/debug_values + #examples/px4_mavlink_debug + + # Tutorial code from + # https://px4.io/dev/example_fixedwing_control + #examples/fixedwing_control + + # Hardware test + #examples/hwtest +) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +set(config_extra_libs + ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a + uavcan + uavcan_stm32_driver + ) + +set(config_io_extra_libs + #${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a + ) + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + MAIN "sercon" STACK "2048") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + MAIN "serdis" STACK "2048") diff --git a/nuttx-configs/px4fmu-v3/include/board.h b/nuttx-configs/px4fmu-v3/include/board.h new file mode 100755 index 000000000000..54e21bbfe711 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/include/board.h @@ -0,0 +1,295 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (25,000,000 / 25) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * is we set aside more DMA channels/streams. + * + * SDIO DMA + *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + *   DMAMAP_SDIO_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDIO DMAMAP_SDIO_1 + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* ESP8266 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 + +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* UART8 has no alternate pin config */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 + +/* + * CAN + * + * CAN1 is routed to the onboard transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + + +/* + * SPI + * + * There are sensors on SPI1, and SPI2 is connected to the FRAM. + */ +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) + +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h new file mode 100644 index 000000000000..15e4e7a8d5a6 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/px4fmu-v3/nsh/Make.defs b/nuttx-configs/px4fmu-v3/nsh/Make.defs new file mode 100644 index 000000000000..6177f4c36d02 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/Make.defs @@ -0,0 +1,177 @@ +############################################################################ +# configs/px4fmu-v2/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -Os +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wno-sign-compare \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wnested-externs +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4fmu-v3/nsh/appconfig b/nuttx-configs/px4fmu-v3/nsh/appconfig new file mode 100644 index 000000000000..0e18aa8ef10a --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/appconfig @@ -0,0 +1,52 @@ +############################################################################ +# configs/px4fmu/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline + +ifeq ($(CONFIG_CAN),y) +#CONFIGURED_APPS += examples/can +endif + +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx-configs/px4fmu-v3/nsh/defconfig b/nuttx-configs/px4fmu-v3/nsh/defconfig new file mode 100644 index 000000000000..957578233b7c --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/defconfig @@ -0,0 +1,1068 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n + +# +# Subsystem Debug Options +# +# CONFIG_DEBUG_MM is not set +# CONFIG_DEBUG_SCHED is not set +# CONFIG_DEBUG_USB is not set +CONFIG_DEBUG_FS=y +# CONFIG_DEBUG_LIB is not set +# CONFIG_DEBUG_BINFMT is not set +# CONFIG_DEBUG_GRAPHICS is not set + +# +# Driver Debug Options +# +# CONFIG_DEBUG_ANALOG is not set +# CONFIG_DEBUG_I2C is not set +# CONFIG_DEBUG_SPI is not set +# CONFIG_DEBUG_SDIO is not set +# CONFIG_DEBUG_GPIO is not set +CONFIG_DEBUG_DMA=y +# CONFIG_DEBUG_WATCHDOG is not set +# CONFIG_DEBUG_AUDIO is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set +# CONFIG_DEBUG_HARDFAULT is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=n +CONFIG_SERIAL_TERMIOS=y +CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMAPRIO=0x00010000 +# CONFIG_SDIO_WIDTH_D1_ONLY is not set + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_ARCH_CHIP_STM32F427V=y +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +CONFIG_STM32_STM32F427=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_I2C3 is not set +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +CONFIG_STM32_SDIO=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +# CONFIG_STM32_SPI3 is not set +# CONFIG_STM32_SPI4 is not set +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_USART6 is not set +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# Hot fix for lost data +CONFIG_STM32_RXDMA_BUFFER_SIZE_OVERRIDE=256 +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_UART4_RS485 is not set +CONFIG_UART4_RXDMA=y +CONFIG_UART5_RXDMA=y +# CONFIG_USART6_RS485 is not set +# CONFIG_USART6_RXDMA is not set +# CONFIG_UART7_RS485 is not set +CONFIG_UART7_RXDMA=y +# CONFIG_UART8_RS485 is not set +CONFIG_UART8_RXDMA=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# SDIO Configuration +# +CONFIG_SDIO_PRI=128 + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +# CONFIG_ARCH_IRQPRIO is not set +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=262144 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +# The actual usage is 420 bytes +CONFIG_ARCH_INTERRUPTSTACK=750 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4FMU_V3=y +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_MMCSD_HAVE_SDIOWAIT_WRCOMPLETE=y +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=1000 +CONFIG_USERMAIN_STACKSIZE=2500 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +CONFIG_RTC=y +CONFIG_RTC_DATETIME=y +CONFIG_RTC_HSECLOCK=y +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +# CONFIG_MMCSD_SPI is not set +CONFIG_MMCSD_SDIO=y +# CONFIG_SDIO_MUXBUS is not set +# CONFIG_SDIO_BLOCKSETUP is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +CONFIG_MTD_RAMTRON=y +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART4=y +CONFIG_ARCH_HAVE_UART7=y +CONFIG_ARCH_HAVE_UART8=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_SERIAL_TIOCSERGSTRUCT is not set +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_UART4_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +CONFIG_UART7_SERIAL_CONSOLE=y +# CONFIG_UART8_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=128 +CONFIG_USART1_TXBUFSIZE=32 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=300 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y + +# +# UART4 Configuration +# +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_TXBUFSIZE=300 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_BITS=8 +CONFIG_UART4_PARITY=0 +CONFIG_UART4_2STOP=0 +# CONFIG_UART4_IFLOWCONTROL is not set +# CONFIG_UART4_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_TXBUFSIZE=300 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +# CONFIG_USART6_IFLOWCONTROL is not set +# CONFIG_USART6_OFLOWCONTROL is not set + +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_TXBUFSIZE=300 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set + +# +# UART8 Configuration +# +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_TXBUFSIZE=300 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_BITS=8 +CONFIG_UART8_PARITY=0 +CONFIG_UART8_2STOP=0 +# CONFIG_UART8_IFLOWCONTROL is not set +# CONFIG_UART8_OFLOWCONTROL is not set +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +# CONFIG_CDCACM_CONSOLE is not set +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=4000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set +# CONFIG_DEBUG_GRAN is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=1600 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=1600 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_USBDEV_TRACE is not set +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# +# CONFIG_SYSTEM_FLASH_ERASEALL is not set + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# + +CONFIG_NSOCKET_DESCRIPTORS=0 diff --git a/nuttx-configs/px4fmu-v3/nsh/setenv.sh b/nuttx-configs/px4fmu-v3/nsh/setenv.sh new file mode 100755 index 000000000000..265520997da2 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v3/scripts/ld.script b/nuttx-configs/px4fmu-v3/scripts/ld.script new file mode 100644 index 000000000000..3649227a43dc --- /dev/null +++ b/nuttx-configs/px4fmu-v3/scripts/ld.script @@ -0,0 +1,159 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + /* 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 +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v3/src/Makefile b/nuttx-configs/px4fmu-v3/src/Makefile new file mode 100644 index 000000000000..a93664b08ba8 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/px4fmu-v3/src/empty.c b/nuttx-configs/px4fmu-v3/src/empty.c new file mode 100644 index 000000000000..5de10699fbb4 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/src/drivers/boards/px4fmu-v3/CMakeLists.txt b/src/drivers/boards/px4fmu-v3/CMakeLists.txt new file mode 100644 index 000000000000..e83c7fd7fda7 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__px4fmu-v3 + COMPILE_FLAGS + -Os + SRCS + px4fmu_can.c + px4fmu3_init.c + px4fmu_pwm_servo.c + px4fmu_spi.c + px4fmu_usb.c + px4fmu3_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4fmu-v3/board_config.h b/src/drivers/boards/px4fmu-v3/board_config.h new file mode 100644 index 000000000000..d2cea93b87e1 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/board_config.h @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * PX4FMUv2 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +#define UDID_START 0x1FFF7A10 + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ +//{GPIO_RSSI_IN, 0, 0}, - pio Analog used as PWM +//{0, GPIO_LED_SAFETY, 0}, pio replacement +//{GPIO_SAFETY_SWITCH_IN, 0, 0}, pio replacement +//{0, GPIO_PERIPH_3V3_EN, 0}, Owned by the 8266 driver +//{0, GPIO_SBUS_INV, 0}, https://github.com/PX4/Firmware/blob/master/src/modules/px4iofirmware/sbus.c +//{GPIO_8266_GPIO0, 0, 0}, Owned by the 8266 driver +//{0, GPIO_SPEKTRUM_POWER, 0}, Owned Spektum driver input to auto pilot +//{0, GPIO_8266_PD, 0}, Owned by the 8266 driver +//{0, GPIO_8266_RST, 0}, Owned by the 8266 driver + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3) + +#define GPIO_LED_RED GPIO_LED1 +#define GPIO_LED_GREEN GPIO_LED2 +#define GPIO_LED_BLUE GPIO_LED3 + +/* Define the Chip Selects */ + +#define GPIO_SPI_CS_MPU9250 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_HMC5983 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) +#define GPIO_SPI_CS_MS5611 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_ICM_20608_G (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) + +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* Define the Ready interrupts */ + +#define GPIO_DRDY_MPU9250 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) +#define GPIO_DRDY_HMC5983 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN12) +#define GPIO_DRDY_ICM_20608_G (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14) + + +/* + * Define the ability to shut off off the sensor signals + * by changing the signals to inputs + */ + +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) + +#define GPIO_SPI_CS_OFF_MPU9250 _PIN_OFF(GPIO_SPI_CS_MPU9250) +#define GPIO_SPI_CS_OFF_HMC5983 _PIN_OFF(GPIO_SPI_CS_HMC5983) +#define GPIO_SPI_CS_OFF_MS5611 _PIN_OFF(GPIO_SPI_CS_MS5611) +#define GPIO_SPI_CS_OFF_ICM_20608_G _PIN_OFF(GPIO_SPI_CS_ICM_20608_G) + +#define GPIO_DRDY_OFF_MPU9250 _PIN_OFF(GPIO_DRDY_MPU9250) +#define GPIO_DRDY_OFF_HMC5983 _PIN_OFF(GPIO_DRDY_HMC5983) +#define GPIO_DRDY_OFF_ICM_20608_G _PIN_OFF(GPIO_DRDY_ICM_20608_G) + + +/* SPI1 off */ +#define GPIO_SPI1_SCK_OFF _PIN_OFF(GPIO_SPI1_SCK) +#define GPIO_SPI1_MISO_OFF _PIN_OFF(GPIO_SPI1_MISO) +#define GPIO_SPI1_MOSI_OFF _PIN_OFF(GPIO_SPI1_MOSI) + +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_RAMTRON 2 + +/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_BARO 3 +#define PX4_SPIDEV_MPU 4 +#define PX4_SPIDEV_HMC 5 + +/* I2C busses */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION + +/* Devices on the external bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + */ +#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15) + +// ADC defines to be used in sensors.cpp to read from a particular channel +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 + +/* User GPIOs + * + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14) + +/* Power supply control and monitoring GPIOs */ +#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/* PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel */ + +#define HRT_PPM_CHANNEL 2 /* use capture/compare channel 2 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF3|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN7) + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL 2 +#define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 + +#define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) +#define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) +#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) + +#define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) +#define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_8266_PD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) +#define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_init.c b/src/drivers/boards/px4fmu-v3/px4fmu3_init.c new file mode 100644 index 000000000000..a1e53d41c55d --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu3_init.c @@ -0,0 +1,344 @@ +/**************************************************************************** + * + * 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + +#else + +# define dma_alloc_init() + +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct sdio_dev_s *sdio; + +#include + +/* TODO XXX commented this out to get cmake build working */ +/*#ifdef __cplusplus*/ +/*__EXPORT int matherr(struct __exception *e)*/ +/*{*/ +/*return 1;*/ +/*}*/ +/*#else*/ +/*__EXPORT int matherr(struct exception *e)*/ +/*{*/ +/*return 1;*/ +/*}*/ +/*#endif*/ + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ + // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ + + /* configure power supply control/sense pins */ + stm32_configgpio(GPIO_PERIPH_3V3_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_GPIO5_OUTPUT); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure the DMA allocator */ + dma_alloc_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_GREEN); + led_off(LED_BLUE); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\n"); + up_ledon(LED_RED); + return -ENODEV; + } + + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_HMC, false); + SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); + + /* Get the SPI port for the FRAM */ + + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\n"); + up_ledon(LED_RED); + return -ENODEV; + } + + /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) + * and de-assert the known chip selects. */ + + // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated + SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH, false); + +#ifdef CONFIG_MMCSD + /* First, get an instance of the SDIO interface */ + + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + + if (!sdio) { + message("[boot] Failed to initialize SDIO slot %d\n", + CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + + if (ret != OK) { + message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ + sdio_mediachange(sdio, true); + +#endif + + return OK; +} diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_led.c b/src/drivers/boards/px4fmu-v3/px4fmu3_led.c new file mode 100644 index 000000000000..59ed10483e8f --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu3_led.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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 px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + + + +static uint32_t g_ledmap[] = { + GPIO_LED_BLUE, // Indexed by LED_BLUE + GPIO_LED_RED, // Indexed by LED_RED, LED_AMBER + GPIO_LED_SAFETY, // Indexed by LED_SAFETY + GPIO_LED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap)/sizeof(g_ledmap[0])); l++) { + stm32_configgpio(g_ledmap[l]); + } +} + +static void phy_set_led(int led, bool state) +{ + /* Pull Down to switch on */ + stm32_gpiowrite(g_ledmap[led], !state); +} + +static bool phy_get_led(int led) +{ + + return !stm32_gpioread(g_ledmap[led]); +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + + phy_set_led(led, !phy_get_led(led)); +} diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_can.c b/src/drivers/boards/px4fmu-v3/px4fmu_can.c new file mode 100644 index 000000000000..62f170957dba --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif \ No newline at end of file diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c new file mode 100644 index 000000000000..600dfef41224 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_spi.c b/src/drivers/boards/px4fmu-v3/px4fmu_spi.c new file mode 100644 index 000000000000..e0c92c1448ac --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_spi.c @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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 px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_MPU9250); + stm32_configgpio(GPIO_SPI_CS_HMC5983); + stm32_configgpio(GPIO_SPI_CS_MS5611); + stm32_configgpio(GPIO_SPI_CS_ICM_20608_G); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + + stm32_configgpio(GPIO_DRDY_MPU9250); + stm32_configgpio(GPIO_DRDY_HMC5983); + stm32_configgpio(GPIO_DRDY_ICM_20608_G); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); +#endif + +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, !selected); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + case PX4_SPIDEV_HMC: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, !selected); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_usb.c b/src/drivers/boards/px4fmu-v3/px4fmu_usb.c new file mode 100644 index 000000000000..e4cfc27e1562 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index c91724036c28..171f3c4aa98e 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -94,6 +94,29 @@ #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +/* + * PX4FMUv3 GPIO numbers. + * + * There are no alternate functions on this board. + */ +# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */ +# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */ +# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */ +# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */ +# define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ +# define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ + +# define GPIO_3V3_SENSORS_EN (1<<7) /**< PE3 - VDD_3V3_SENSORS_EN */ +# define GPIO_BRICK_VALID (1<<8) /**< PB5 - !VDD_BRICK_VALID */ + +/** + * Device paths for things that support the GPIO ioctl protocol. + */ +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" + +#endif + #ifdef CONFIG_ARCH_BOARD_AEROCORE /* * AeroCore GPIO numbers and configuration. @@ -121,7 +144,7 @@ #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ - !defined(CONFIG_ARCH_BOARD_POSIXTEST) + !defined(CONFIG_ARCH_BOARD_PX4FMU_V3) && !defined(CONFIG_ARCH_BOARD_SITL) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index dbcfde91fa40..3daafc963fb2 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -52,6 +52,8 @@ #define LED_RED 1 /* some boards have red rather than amber */ #define LED_BLUE 0 #define LED_SAFETY 2 +#define LED_GREEN 3 + #define LED_ON _PX4_IOC(_LED_BASE, 0) #define LED_OFF _PX4_IOC(_LED_BASE, 1) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7b42f82061f8..f9fd009cf74e 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -331,7 +331,7 @@ MEASAirspeed::cycle() void MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); } @@ -374,7 +374,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) voltage_diff = -1.0f; } temperature -= voltage_diff * temp_slope; -#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) } /** diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b7538d270de0..6deecdc168e5 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -120,7 +120,7 @@ class PX4FMU : public device::CDev #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) static const unsigned _max_actuators = 4; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) static const unsigned _max_actuators = 6; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -224,6 +224,17 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V3) + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, + {GPIO_VDD_BRICK_VALID, 0, 0}, +#endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) /* AeroCore breaks out User GPIOs on J11 */ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 465ee694a674..70962cd89499 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -335,6 +335,40 @@ ADC::update_system_power(void) _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); } #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 + system_power_s system_power; + system_power.timestamp = hrt_absolute_time(); + + system_power.voltage5V_v = 0; + + for (unsigned i = 0; i < _channel_count; i++) { + if (_samples[i].am_channel == 4) { + // it is 2:1 scaled + system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); + } + } + + // these are not ADC related, but it is convenient to + // publish these to the same topic + system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS); + + // note that the valid pins are active High + system_power.brick_valid = stm32_gpioread(GPIO_VDD_BRICK_VALID); + system_power.servo_valid = 1; + + // OC pins are not supported + system_power.periph_5V_OC = 0; + system_power.hipower_5V_OC = 0; + + /* lazily publish */ + if (_to_system_power != nullptr) { + orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + + } else { + _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); + } + +#endif // CONFIG_ARCH_BOARD_PX4FMU_V3 } uint16_t diff --git a/src/lib/version/version.h b/src/lib/version/version.h index ddf13822df83..c6daa3ec934c 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -51,6 +51,10 @@ #define HW_ARCH "PX4FMU_V2" #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +#define HW_ARCH "PX4FMU_V3" +#endif + #ifdef CONFIG_ARCH_BOARD_AEROCORE #define HW_ARCH "AEROCORE" #endif From 7bd570a8033f912031601eb4cb16b9e3d3639bb9 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 04:50:53 -1000 Subject: [PATCH 175/293] Renamed pax4fmu-v3 to pax4fmu-v4 --- Images/{px4fmu-v3.prototype => px4fmu-v4.prototype} | 4 ++-- ...x4fmu-v3_default.cmake => nuttx_px4fmu-v4_default.cmake} | 2 +- nuttx-configs/{px4fmu-v3 => px4fmu-v4}/include/board.h | 0 .../{px4fmu-v3 => px4fmu-v4}/include/nsh_romfsimg.h | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/Make.defs | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/appconfig | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/defconfig | 2 +- nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/setenv.sh | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/scripts/ld.script | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/Makefile | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/empty.c | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/CMakeLists.txt | 6 +++--- src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/board_config.h | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_can.c | 0 .../{px4fmu-v3/px4fmu3_init.c => px4fmu-v4/px4fmu_init.c} | 0 .../{px4fmu-v3/px4fmu3_led.c => px4fmu-v4/px4fmu_led.c} | 0 .../boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_pwm_servo.c | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_spi.c | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_usb.c | 0 src/drivers/drv_gpio.h | 4 ++-- src/drivers/meas_airspeed/meas_airspeed.cpp | 5 +++-- src/drivers/px4fmu/fmu.cpp | 4 ++-- src/drivers/stm32/adc/adc.cpp | 4 ++-- src/lib/version/version.h | 4 ++-- src/modules/gpio_led/gpio_led.c | 6 +++--- 25 files changed, 21 insertions(+), 20 deletions(-) rename Images/{px4fmu-v3.prototype => px4fmu-v4.prototype} (68%) rename cmake/configs/{nuttx_px4fmu-v3_default.cmake => nuttx_px4fmu-v4_default.cmake} (99%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/include/board.h (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/include/nsh_romfsimg.h (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/Make.defs (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/appconfig (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/defconfig (99%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/setenv.sh (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/scripts/ld.script (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/Makefile (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/empty.c (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/CMakeLists.txt (96%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/board_config.h (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_can.c (100%) rename src/drivers/boards/{px4fmu-v3/px4fmu3_init.c => px4fmu-v4/px4fmu_init.c} (100%) rename src/drivers/boards/{px4fmu-v3/px4fmu3_led.c => px4fmu-v4/px4fmu_led.c} (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_pwm_servo.c (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_spi.c (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_usb.c (100%) diff --git a/Images/px4fmu-v3.prototype b/Images/px4fmu-v4.prototype similarity index 68% rename from Images/px4fmu-v3.prototype rename to Images/px4fmu-v4.prototype index ada86b9a93ff..24e72f868dea 100644 --- a/Images/px4fmu-v3.prototype +++ b/Images/px4fmu-v4.prototype @@ -1,10 +1,10 @@ { "board_id": 11, "magic": "PX4FWv1", - "description": "Firmware for the PX4FMUv3 board", + "description": "Firmware for the PX4FMUv4 board", "image": "", "build_time": 0, - "summary": "PX4FMUv3", + "summary": "PX4FMUv4", "version": "0.1", "image_size": 0, "git_identity": "", diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake similarity index 99% rename from cmake/configs/nuttx_px4fmu-v3_default.cmake rename to cmake/configs/nuttx_px4fmu-v4_default.cmake index 82f4c1b9a799..cc1db95eef55 100644 --- a/cmake/configs/nuttx_px4fmu-v3_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -12,7 +12,7 @@ set(config_module_list drivers/stm32/tone_alarm drivers/led drivers/px4fmu - drivers/boards/px4fmu-v3 + drivers/boards/px4fmu-v4 drivers/rgbled drivers/mpu9250 drivers/hmc5883 diff --git a/nuttx-configs/px4fmu-v3/include/board.h b/nuttx-configs/px4fmu-v4/include/board.h similarity index 100% rename from nuttx-configs/px4fmu-v3/include/board.h rename to nuttx-configs/px4fmu-v4/include/board.h diff --git a/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v4/include/nsh_romfsimg.h similarity index 100% rename from nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h rename to nuttx-configs/px4fmu-v4/include/nsh_romfsimg.h diff --git a/nuttx-configs/px4fmu-v3/nsh/Make.defs b/nuttx-configs/px4fmu-v4/nsh/Make.defs similarity index 100% rename from nuttx-configs/px4fmu-v3/nsh/Make.defs rename to nuttx-configs/px4fmu-v4/nsh/Make.defs diff --git a/nuttx-configs/px4fmu-v3/nsh/appconfig b/nuttx-configs/px4fmu-v4/nsh/appconfig similarity index 100% rename from nuttx-configs/px4fmu-v3/nsh/appconfig rename to nuttx-configs/px4fmu-v4/nsh/appconfig diff --git a/nuttx-configs/px4fmu-v3/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig similarity index 99% rename from nuttx-configs/px4fmu-v3/nsh/defconfig rename to nuttx-configs/px4fmu-v4/nsh/defconfig index 957578233b7c..551a9cd4ff67 100644 --- a/nuttx-configs/px4fmu-v3/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -384,7 +384,7 @@ CONFIG_BOOT_RUNFROMFLASH=y # # Board Selection # -CONFIG_ARCH_BOARD_PX4FMU_V3=y +CONFIG_ARCH_BOARD_PX4FMU_V4=y CONFIG_ARCH_BOARD_CUSTOM=y CONFIG_ARCH_BOARD="" diff --git a/nuttx-configs/px4fmu-v3/nsh/setenv.sh b/nuttx-configs/px4fmu-v4/nsh/setenv.sh similarity index 100% rename from nuttx-configs/px4fmu-v3/nsh/setenv.sh rename to nuttx-configs/px4fmu-v4/nsh/setenv.sh diff --git a/nuttx-configs/px4fmu-v3/scripts/ld.script b/nuttx-configs/px4fmu-v4/scripts/ld.script similarity index 100% rename from nuttx-configs/px4fmu-v3/scripts/ld.script rename to nuttx-configs/px4fmu-v4/scripts/ld.script diff --git a/nuttx-configs/px4fmu-v3/src/Makefile b/nuttx-configs/px4fmu-v4/src/Makefile similarity index 100% rename from nuttx-configs/px4fmu-v3/src/Makefile rename to nuttx-configs/px4fmu-v4/src/Makefile diff --git a/nuttx-configs/px4fmu-v3/src/empty.c b/nuttx-configs/px4fmu-v4/src/empty.c similarity index 100% rename from nuttx-configs/px4fmu-v3/src/empty.c rename to nuttx-configs/px4fmu-v4/src/empty.c diff --git a/src/drivers/boards/px4fmu-v3/CMakeLists.txt b/src/drivers/boards/px4fmu-v4/CMakeLists.txt similarity index 96% rename from src/drivers/boards/px4fmu-v3/CMakeLists.txt rename to src/drivers/boards/px4fmu-v4/CMakeLists.txt index e83c7fd7fda7..b16a9913313a 100644 --- a/src/drivers/boards/px4fmu-v3/CMakeLists.txt +++ b/src/drivers/boards/px4fmu-v4/CMakeLists.txt @@ -31,16 +31,16 @@ # ############################################################################ px4_add_module( - MODULE drivers__boards__px4fmu-v3 + MODULE drivers__boards__px4fmu-v4 COMPILE_FLAGS -Os SRCS px4fmu_can.c - px4fmu3_init.c + px4fmu_init.c px4fmu_pwm_servo.c px4fmu_spi.c px4fmu_usb.c - px4fmu3_led.c + px4fmu_led.c DEPENDS platforms__common ) diff --git a/src/drivers/boards/px4fmu-v3/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h similarity index 100% rename from src/drivers/boards/px4fmu-v3/board_config.h rename to src/drivers/boards/px4fmu-v4/board_config.h diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_can.c b/src/drivers/boards/px4fmu-v4/px4fmu_can.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_can.c rename to src/drivers/boards/px4fmu-v4/px4fmu_can.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu3_init.c rename to src/drivers/boards/px4fmu-v4/px4fmu_init.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_led.c b/src/drivers/boards/px4fmu-v4/px4fmu_led.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu3_led.c rename to src/drivers/boards/px4fmu-v4/px4fmu_led.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c rename to src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_spi.c b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_spi.c rename to src/drivers/boards/px4fmu-v4/px4fmu_spi.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_usb.c b/src/drivers/boards/px4fmu-v4/px4fmu_usb.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_usb.c rename to src/drivers/boards/px4fmu-v4/px4fmu_usb.c diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 171f3c4aa98e..cc378472692f 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -94,7 +94,7 @@ #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 /* * PX4FMUv3 GPIO numbers. * @@ -144,7 +144,7 @@ #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ - !defined(CONFIG_ARCH_BOARD_PX4FMU_V3) && !defined(CONFIG_ARCH_BOARD_SITL) + !defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index f9fd009cf74e..ac3339f38c27 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -331,7 +331,8 @@ MEASAirspeed::cycle() void MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); } @@ -374,7 +375,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) voltage_diff = -1.0f; } temperature -= voltage_diff * temp_slope; -#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) } /** diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 6deecdc168e5..6b0f75d93c8d 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -120,7 +120,7 @@ class PX4FMU : public device::CDev #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) static const unsigned _max_actuators = 4; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) static const unsigned _max_actuators = 6; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -224,7 +224,7 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 70962cd89499..a758413df76e 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -335,7 +335,7 @@ ADC::update_system_power(void) _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); } #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 system_power_s system_power; system_power.timestamp = hrt_absolute_time(); @@ -368,7 +368,7 @@ ADC::update_system_power(void) _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); } -#endif // CONFIG_ARCH_BOARD_PX4FMU_V3 +#endif // CONFIG_ARCH_BOARD_PX4FMU_V4 } uint16_t diff --git a/src/lib/version/version.h b/src/lib/version/version.h index c6daa3ec934c..e63b1bd6b9db 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -51,8 +51,8 @@ #define HW_ARCH "PX4FMU_V2" #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 -#define HW_ARCH "PX4FMU_V3" +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 +#define HW_ARCH "PX4FMU_V4" #endif #ifdef CONFIG_ARCH_BOARD_AEROCORE diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index f8c62a88a73f..87fbfbf4aeb6 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -89,7 +89,7 @@ int gpio_led_main(int argc, char *argv[]) "\t\tr2\tPX4IO RELAY2" ); #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) errx(1, "usage: gpio_led {start|stop} [-p ]\n" "\t-p \tUse specified AUX OUT pin number (default: 1)" ); @@ -111,7 +111,7 @@ int gpio_led_main(int argc, char *argv[]) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 char *pin_name = "PX4FMU GPIO_EXT1"; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) char pin_name[] = "AUX OUT 1"; #endif @@ -154,7 +154,7 @@ int gpio_led_main(int argc, char *argv[]) } #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) unsigned int n = strtoul(argv[3], NULL, 10); if (n >= 1 && n <= 6) { From 58998b9f370390197d1d986479450d8cbda837b3 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 12:13:24 -1000 Subject: [PATCH 176/293] Added support for ICM-20608-G to MPU6000 driver --- src/drivers/mpu6000/mpu6000.cpp | 80 +++++++++++++++++++++++++++------ 1 file changed, 67 insertions(+), 13 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1b300194b6a8..6059096078df 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -148,6 +148,12 @@ #define BIT_INT_STATUS_DATA 0x01 #define MPU_WHOAMI_6000 0x68 +#define ICM_WHOAMI_20608 0xaf + +// Product ID Description for ICM2608 +// There is none + +#define ICM20608_REV_00 0 // Product ID Description for MPU6000 // high 4 bits low 4 bits @@ -204,7 +210,8 @@ class MPU6000_gyro; class MPU6000 : public device::SPI { public: - MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation); + MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation, + int device_type); virtual ~MPU6000(); virtual int init(); @@ -238,6 +245,7 @@ class MPU6000 : public device::SPI virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); private: + int _device_type; MPU6000_gyro *_gyro; uint8_t _product; /** product code */ @@ -321,6 +329,15 @@ class MPU6000 : public device::SPI */ int reset(); + /** + * is_icm_device + */ + bool is_icm_device() { return _device_type == 20608;} + /** + * is_mpu_device + */ + bool is_mpu_device() { return _device_type == 6000;} + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -501,8 +518,10 @@ class MPU6000_gyro : public device::CDev /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) : +MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation, + int device_type) : SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + _device_type(device_type), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), _call{}, @@ -760,15 +779,15 @@ int MPU6000::reset() int MPU6000::probe() { - uint8_t whoami; - whoami = read_reg(MPUREG_WHOAMI); - if (whoami != MPU_WHOAMI_6000) { + uint8_t whoami = read_reg(MPUREG_WHOAMI); + uint8_t expected = is_mpu_device() ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608; + + if (whoami != expected) { debug("unexpected WHOAMI 0x%02x", whoami); return -EIO; - } - /* look for a product ID we recognise */ + /* look for a product ID we recognize */ _product = read_reg(MPUREG_PRODUCT_ID); // verify product revision @@ -785,6 +804,7 @@ MPU6000::probe() case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: + case ICM20608_REV_00: debug("ID 0x%02x", _product); _checked_values[0] = _product; return OK; @@ -1424,6 +1444,13 @@ MPU6000::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; + // There is no MPUREG_PRODUCT_ID on the icm device + // so lets make dummy it up and allow the rest of the + // code to run as is + if (reg == MPUREG_PRODUCT_ID && is_icm_device()) { + return ICM20608_REV_00; + } + // general register transfer at low clock speed set_frequency(speed); @@ -1946,7 +1973,7 @@ namespace mpu6000 MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus -void start(bool, enum Rotation); +void start(bool, enum Rotation, int range, int device_type); void stop(bool); void test(bool); void reset(bool); @@ -1963,7 +1990,7 @@ void usage(); * or failed to detect the sensor. */ void -start(bool external_bus, enum Rotation rotation) +start(bool external_bus, enum Rotation rotation, int range, int device_type) { int fd; MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; @@ -1977,12 +2004,22 @@ start(bool external_bus, enum Rotation rotation) /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); +# if defined(PX4_SPIDEV_EXT_ICM) + spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM); +# else + spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU; +# endif + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, cs, rotation, device_type); #else errx(0, "External SPI not available"); #endif } else { - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); +#if defined(PX4_SPIDEV_ICM) + spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM); +#else + spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_MPU; +#endif + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, cs, rotation, device_type); } if (*g_dev_ptr == nullptr) @@ -2000,6 +2037,10 @@ start(bool external_bus, enum Rotation rotation) if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + if (ioctl(fd, ACCELIOCSRANGE, range) < 0) { + goto fail; + } + close(fd); exit(0); @@ -2195,6 +2236,7 @@ usage() warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -M 6000|20608 (default 6000)"); warnx(" -R rotation"); } @@ -2204,18 +2246,30 @@ int mpu6000_main(int argc, char *argv[]) { bool external_bus = false; + int device_type = 6000; int ch; enum Rotation rotation = ROTATION_NONE; + int accel_range = 16; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XR:")) != EOF) { + while ((ch = getopt(argc, argv, "T:XR:a:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + + case 'T': + device_type = atoi(optarg); + break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + + case 'a': + accel_range = atoi(optarg); + break; + default: mpu6000::usage(); exit(0); @@ -2229,7 +2283,7 @@ mpu6000_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { - mpu6000::start(external_bus, rotation); + mpu6000::start(external_bus, rotation, accel_range, device_type); } if (!strcmp(verb, "stop")) { From 40383d322a545fc682463be151111c942672b57f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 13:58:57 -1000 Subject: [PATCH 177/293] Added missing conditional compilation control for FMUV4 --- src/drivers/px4fmu/fmu.cpp | 122 ++++++++++++++++++++++++++++++++++--- 1 file changed, 112 insertions(+), 10 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 6b0f75d93c8d..f7ad3892badc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1221,10 +1221,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) set_mode(MODE_4PWM); break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - case 6: - set_mode(MODE_6PWM); - break; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + + case 6: + set_mode(MODE_6PWM); + break; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) case 8: @@ -1421,6 +1422,79 @@ PX4FMU::sensor_reset(int ms) // stm32_configgpio(GPIO_ACCEL_DRDY); // stm32_configgpio(GPIO_EXTI_MPU_DRDY); +#endif +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + + if (ms < 1) { + ms = 1; + } + + /* disable SPI bus */ + stm32_configgpio(GPIO_SPI_CS_OFF_MPU9250); + stm32_configgpio(GPIO_SPI_CS_OFF_HMC5983); + stm32_configgpio(GPIO_SPI_CS_OFF_MS5611); + stm32_configgpio(GPIO_SPI_CS_OFF_ICM_20608_G); + + stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 0); + + stm32_configgpio(GPIO_SPI1_SCK_OFF); + stm32_configgpio(GPIO_SPI1_MISO_OFF); + stm32_configgpio(GPIO_SPI1_MOSI_OFF); + + stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); + + stm32_configgpio(GPIO_DRDY_OFF_MPU9250); + stm32_configgpio(GPIO_DRDY_OFF_HMC5983); + stm32_configgpio(GPIO_DRDY_OFF_ICM_20608_G); + + stm32_gpiowrite(GPIO_DRDY_OFF_MPU9250, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_HMC5983, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_ICM_20608_G, 0); + + /* set the sensor rail off */ + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the sensor rail back on */ + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_MPU9250); + stm32_configgpio(GPIO_SPI_CS_HMC5983); + stm32_configgpio(GPIO_SPI_CS_MS5611); + stm32_configgpio(GPIO_SPI_CS_ICM_20608_G); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 1); + + // // XXX bring up the EXTI pins again + // stm32_configgpio(GPIO_GYRO_DRDY); + // stm32_configgpio(GPIO_MAG_DRDY); + // stm32_configgpio(GPIO_ACCEL_DRDY); + // stm32_configgpio(GPIO_EXTI_MPU_DRDY); + #endif #endif } @@ -1447,6 +1521,31 @@ PX4FMU::peripheral_reset(int ms) /* switch the peripheral rail back on */ stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); #endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + + if (ms < 1) { + ms = 10; + } + + /* set the peripheral rails off */ + stm32_configgpio(GPIO_PERIPH_3V3_EN); + + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0); + + bool last = stm32_gpioread(GPIO_SPEKTRUM_POWER); + /* Keep Spektum on to discharge rail*/ + stm32_gpiowrite(GPIO_SPEKTRUM_POWER, 1); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + stm32_gpiowrite(GPIO_SPEKTRUM_POWER, last); + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); +#endif } void @@ -1645,7 +1744,7 @@ fmu_new_mode(PortMode new_mode) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) servo_mode = PX4FMU::MODE_6PWM; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -1653,7 +1752,8 @@ fmu_new_mode(PortMode new_mode) #endif break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + case PORT_PWM4: /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; @@ -1950,7 +2050,8 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + } else if (!strcmp(verb, "mode_pwm4")) { new_mode = PORT_PWM4; #endif @@ -2032,9 +2133,10 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE) - fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds], i2c \n"); + fprintf(stderr, + " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n"); +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) || defined(CONFIG_ARCH_BOARD_AEROCORE) + fprintf(stderr, " mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c \n"); #endif exit(1); } From b0a011546634f88490f2a1645af36e06fe431665 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 15:30:44 -1000 Subject: [PATCH 178/293] Lower the uOrb Priority of the mpu9250 --- src/drivers/mpu9250/mpu9250.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 61dc1673b5df..0a9937a36048 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -656,7 +656,7 @@ MPU9250::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_accel_topic == nullptr) { warnx("ADVERT FAIL"); @@ -668,7 +668,7 @@ MPU9250::init() _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_gyro->_gyro_topic == nullptr) { warnx("ADVERT FAIL"); From b96f8f7cc019829dad00056bfc7cb87eb5e512a9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Nov 2015 09:46:15 +1100 Subject: [PATCH 179/293] build: added makefile fragment for FMUv4 --- makefiles/nuttx/board_px4fmu-v4.mk | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 makefiles/nuttx/board_px4fmu-v4.mk diff --git a/makefiles/nuttx/board_px4fmu-v4.mk b/makefiles/nuttx/board_px4fmu-v4.mk new file mode 100644 index 000000000000..2bc9e3e9ea01 --- /dev/null +++ b/makefiles/nuttx/board_px4fmu-v4.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the PX4FMUv4 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM4F +CONFIG_BOARD = PX4FMU_V4 + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk From dd6975c26847b2c67d56943005270c69752ea38d Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 12:19:35 -1000 Subject: [PATCH 180/293] px4fmu-v4 uses MPU6000 driver for ICM-20609-G --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + src/drivers/boards/px4fmu-v4/board_config.h | 1 + src/drivers/boards/px4fmu-v4/px4fmu_spi.c | 2 +- 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index cc1db95eef55..0b873ccb98f2 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -14,6 +14,7 @@ set(config_module_list drivers/px4fmu drivers/boards/px4fmu-v4 drivers/rgbled + drivers/mpu6000 drivers/mpu9250 drivers/hmc5883 drivers/ms5611 diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index d2cea93b87e1..1fa6f0f2d6f7 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -127,6 +127,7 @@ __BEGIN_DECLS #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 #define PX4_SPIDEV_HMC 5 +#define PX4_SPIDEV_ICM 6 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c index e0c92c1448ac..bdfa47f62c24 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c @@ -101,7 +101,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, /* SPI select is active low, so write !selected to select the device */ switch (devid) { - case PX4_SPIDEV_GYRO: + case PX4_SPIDEV_ICM: /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); From dcf033cf6e066d65c6cc9981dbddae447d196c57 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Nov 2015 12:28:17 +1100 Subject: [PATCH 181/293] px4flow: fixed build with no I2C_BUS_ONBOARD --- src/drivers/px4flow/px4flow.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 0704f16c91e2..85e0e1da775b 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -678,6 +678,7 @@ start() #endif delete g_dev; +#ifdef PX4_I2C_BUS_ONBOARD /* try 3rd bus */ g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD); @@ -688,7 +689,7 @@ start() if (OK != g_dev->init()) { goto fail; } - +#endif #ifdef PX4_I2C_BUS_ESC } #endif From 7ef19b64d6a045a3a84bf4e01fbd9a7a985c757e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Nov 2015 12:28:31 +1100 Subject: [PATCH 182/293] ll40ls: fixed build with no I2C_BUS_ONBOARD --- src/drivers/ll40ls/ll40ls.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 22cebd207cb6..165fc3c96540 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -95,7 +95,7 @@ void usage(); LidarLite * get_dev(const bool use_i2c, const int bus) { LidarLite * g_dev = nullptr; if (use_i2c) { - g_dev = static_cast(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + g_dev = static_cast(bus == PX4_I2C_BUS_EXPANSION ? g_dev_ext : g_dev_int); if (g_dev == nullptr) { errx(1, "i2c driver not running"); } @@ -221,7 +221,7 @@ void start(const bool use_i2c, const int bus) fail: - if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { + if (g_dev_int != nullptr && (bus == -1 || bus != PX4_I2C_BUS_EXPANSION)) { delete g_dev_int; g_dev_int = nullptr; } @@ -245,7 +245,7 @@ void start(const bool use_i2c, const int bus) void stop(const bool use_i2c, const int bus) { if (use_i2c) { - if (bus == PX4_I2C_BUS_ONBOARD) { + if (bus != PX4_I2C_BUS_EXPANSION) { if (g_dev_int != nullptr) { delete g_dev_int; g_dev_int = nullptr; @@ -280,7 +280,7 @@ test(const bool use_i2c, const int bus) const char *path; if (use_i2c) { - path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT); } else { path = LL40LS_DEVICE_PATH_PWM; @@ -351,7 +351,7 @@ reset(const bool use_i2c, const int bus) const char *path; if (use_i2c) { - path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT); } else { path = LL40LS_DEVICE_PATH_PWM; From 04e4075601f1e0a27b1f8759991c483c4b8f96e6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Nov 2015 12:29:00 +1100 Subject: [PATCH 183/293] HACK: disable auto dependency on io firmware --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 359241eb0047..36e473c0e28f 100644 --- a/Makefile +++ b/Makefile @@ -161,7 +161,7 @@ define FMU_DEP $(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4 endef FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS)) -$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) +# $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) # # Build the NuttX export archives. From 0320318ec60c38509a54cf90740da628cb33808e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Nov 2015 12:56:27 +1100 Subject: [PATCH 184/293] adc: imported adc driver from upstream --- src/drivers/stm32/adc/adc.cpp | 61 ++++++++++++++++++++--------------- 1 file changed, 35 insertions(+), 26 deletions(-) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index a758413df76e..164b36ab8980 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -115,7 +115,7 @@ class ADC : public device::CDev private: static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - + hrt_call _call; perf_counter_t _sample_perf; @@ -161,11 +161,13 @@ ADC::ADC(uint32_t channels) : _channel_count++; } } + _samples = new adc_msg_s[_channel_count]; /* prefill the channel numbers in the sample array */ if (_samples != nullptr) { unsigned index = 0; + for (unsigned i = 0; i < 32; i++) { if (channels & (1 << i)) { _samples[index].am_channel = i; @@ -178,8 +180,9 @@ ADC::ADC(uint32_t channels) : ADC::~ADC() { - if (_samples != nullptr) + if (_samples != nullptr) { delete _samples; + } } int @@ -189,8 +192,11 @@ ADC::init() #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_CAL; usleep(100); - if (rCR2 & ADC_CR2_CAL) + + if (rCR2 & ADC_CR2_CAL) { return -1; + } + #endif /* arbitrarily configure all channels for 55 cycle sample time */ @@ -201,7 +207,7 @@ ADC::init() rCR1 = 0; /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = + rCR2 = #ifdef ADC_CR2_TSVREFE /* enable the temperature sensor in CR2 */ ADC_CR2_TSVREFE | @@ -216,7 +222,7 @@ ADC::init() /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ + rSQR3 = 0; /* will be updated with the channel each tick */ /* power-cycle the ADC and turn it on */ rCR2 &= ~ADC_CR2_ADON; @@ -229,6 +235,7 @@ ADC::init() /* kick off a sample and wait for it to complete */ hrt_abstime now = hrt_absolute_time(); rCR2 |= ADC_CR2_SWSTART; + while (!(rSR & ADC_SR_EOC)) { /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ @@ -256,8 +263,9 @@ ADC::read(file *filp, char *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; - if (len > maxsize) + if (len > maxsize) { len = maxsize; + } /* block interrupts while copying samples to avoid racing with an update */ irqstate_t flags = irqsave(); @@ -296,8 +304,10 @@ void ADC::_tick() { /* scan the channel set and sample each */ - for (unsigned i = 0; i < _channel_count; i++) + for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); + } + update_system_power(); } @@ -309,6 +319,7 @@ ADC::update_system_power(void) system_power.timestamp = hrt_absolute_time(); system_power.voltage5V_v = 0; + for (unsigned i = 0; i < _channel_count; i++) { if (_samples[i].am_channel == 4) { // it is 2:1 scaled @@ -331,9 +342,11 @@ ADC::update_system_power(void) /* lazily publish */ if (_to_system_power != nullptr) { orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + } else { _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); } + #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 system_power_s system_power; @@ -377,8 +390,9 @@ ADC::_sample(unsigned channel) perf_begin(_sample_perf); /* clear any previous EOC */ - if (rSR & ADC_SR_EOC) + if (rSR & ADC_SR_EOC) { rSR &= ~ADC_SR_EOC; + } /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; @@ -386,6 +400,7 @@ ADC::_sample(unsigned channel) /* wait for the conversion to complete */ hrt_abstime now = hrt_absolute_time(); + while (!(rSR & ADC_SR_EOC)) { /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ @@ -416,20 +431,23 @@ test(void) { int fd = open(ADC0_DEVICE_PATH, O_RDONLY); - if (fd < 0) + + if (fd < 0) { err(1, "can't open ADC device"); + } for (unsigned i = 0; i < 50; i++) { adc_msg_s data[12]; ssize_t count = read(fd, data, sizeof(data)); - if (count < 0) + if (count < 0) { errx(1, "read error"); + } unsigned channels = count / sizeof(data[0]); for (unsigned j = 0; j < channels; j++) { - printf ("%d: %u ", data[j].am_channel, data[j].am_data); + printf("%d: %u ", data[j].am_channel, data[j].am_data); } printf("\n"); @@ -444,22 +462,12 @@ int adc_main(int argc, char *argv[]) { if (g_adc == nullptr) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - /* XXX this hardcodes the default channel set for PX4FMUv1 - should be configurable */ - g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); -#endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - /* XXX this hardcodes the default channel set for PX4FMUv2 - should be configurable */ - g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | - (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)); -#endif -#ifdef CONFIG_ARCH_BOARD_AEROCORE - /* XXX this hardcodes the default channel set for AeroCore - should be configurable */ - g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); -#endif + /* XXX this hardcodes the default channel set for the board in board_config.h - should be configurable */ + g_adc = new ADC(ADC_CHANNELS); - if (g_adc == nullptr) + if (g_adc == nullptr) { errx(1, "couldn't allocate the ADC driver"); + } if (g_adc->init() != OK) { delete g_adc; @@ -468,8 +476,9 @@ adc_main(int argc, char *argv[]) } if (argc > 1) { - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { test(); + } } exit(0); From c97b6932aa8ab0ce78655bf1de8fc9e42c11c815 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Nov 2015 13:05:32 +1100 Subject: [PATCH 185/293] FMUv4: added module.mk for board --- src/drivers/boards/px4fmu-v4/module.mk | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 src/drivers/boards/px4fmu-v4/module.mk diff --git a/src/drivers/boards/px4fmu-v4/module.mk b/src/drivers/boards/px4fmu-v4/module.mk new file mode 100644 index 000000000000..6a3efcb533e5 --- /dev/null +++ b/src/drivers/boards/px4fmu-v4/module.mk @@ -0,0 +1,12 @@ +# +# Board-specific startup code for the PX4FMUv2 +# + +SRCS = px4fmu_can.c \ + px4fmu_init.c \ + px4fmu_pwm_servo.c \ + px4fmu_spi.c \ + px4fmu_usb.c \ + px4fmu_led.c + +MAXOPTIMIZATION = -Os From 3eee3e64ddcbd7a576e941e5b5c16e5eb493de55 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Nov 2015 13:05:59 +1100 Subject: [PATCH 186/293] make: ignore generated APM build file --- makefiles/nuttx/.gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 makefiles/nuttx/.gitignore diff --git a/makefiles/nuttx/.gitignore b/makefiles/nuttx/.gitignore new file mode 100644 index 000000000000..7fca5d8ebaf6 --- /dev/null +++ b/makefiles/nuttx/.gitignore @@ -0,0 +1 @@ +/config_px4fmu-v4_APM.mk From 63b57a2d9b3ade1f030f91235b264092f74d7fb9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Nov 2015 18:30:18 +1100 Subject: [PATCH 187/293] px4fmu: support "fmu mode_pwm6" command --- src/drivers/px4fmu/fmu.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index f7ad3892badc..eee184a4bb3f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1717,6 +1717,7 @@ enum PortMode { PORT_PWM_AND_SERIAL, PORT_PWM_AND_GPIO, PORT_PWM4, + PORT_PWM6, }; PortMode g_port_mode; @@ -1758,6 +1759,11 @@ fmu_new_mode(PortMode new_mode) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; break; + + case PORT_PWM6: + /* select 6-pin PWM mode */ + servo_mode = PX4FMU::MODE_6PWM; + break; #endif /* mixed modes supported on v1 board only */ @@ -2054,6 +2060,8 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm4")) { new_mode = PORT_PWM4; + } else if (!strcmp(verb, "mode_pwm6")) { + new_mode = PORT_PWM6; #endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) From b677da20b8f76cdabc35b1ba93b53404792306ab Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Nov 2015 21:32:02 +1100 Subject: [PATCH 188/293] FMUv4: disable TIM3 in defconfig --- nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 551a9cd4ff67..6c05df3b9886 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -241,7 +241,7 @@ CONFIG_STM32_SPI2=y CONFIG_STM32_SYSCFG=y CONFIG_STM32_TIM1=y # CONFIG_STM32_TIM2 is not set -CONFIG_STM32_TIM3=y +# CONFIG_STM32_TIM3 is not set CONFIG_STM32_TIM4=y # CONFIG_STM32_TIM5 is not set # CONFIG_STM32_TIM6 is not set From 822e4ad64738cfeb1247660fbf0283e068c5f5ef Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Nov 2015 21:32:21 +1100 Subject: [PATCH 189/293] FMUv4: switch HRT to TIM3. This fixes PPMSUM input --- src/drivers/boards/px4fmu-v4/board_config.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 1fa6f0f2d6f7..3500d35e613b 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -208,11 +208,11 @@ __BEGIN_DECLS #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) /* High-resolution timer */ -#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER 3 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 4 /* use capture/compare channel */ -#define HRT_PPM_CHANNEL 2 /* use capture/compare channel 2 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF3|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN7) +#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 From 7d5984a13549995577624a6c7e0c2d056c21e3a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:14:06 +0100 Subject: [PATCH 190/293] Move S.BUS and DSM decoders into RC lib --- src/{modules/px4iofirmware => lib/rc}/dsm.c | 0 src/{modules/px4iofirmware => lib/rc}/sbus.c | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename src/{modules/px4iofirmware => lib/rc}/dsm.c (100%) rename src/{modules/px4iofirmware => lib/rc}/sbus.c (100%) diff --git a/src/modules/px4iofirmware/dsm.c b/src/lib/rc/dsm.c similarity index 100% rename from src/modules/px4iofirmware/dsm.c rename to src/lib/rc/dsm.c diff --git a/src/modules/px4iofirmware/sbus.c b/src/lib/rc/sbus.c similarity index 100% rename from src/modules/px4iofirmware/sbus.c rename to src/lib/rc/sbus.c From 399091516aeebd456bf5acc1da0818b574743119 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:14:26 +0100 Subject: [PATCH 191/293] Build S.BUS and DSM decoders in RC lib --- src/lib/rc/dsm.c | 43 ++++++++++++++++++++---------------- src/lib/rc/dsm.h | 52 ++++++++++++++++++++++++++++++++++++++++++++ src/lib/rc/sbus.c | 35 ++++++++++++------------------ src/lib/rc/sbus.h | 55 +++++++++++++++++++++++++++++++++++++++++++++++ src/lib/rc/sumd.h | 5 +++-- 5 files changed, 148 insertions(+), 42 deletions(-) create mode 100644 src/lib/rc/dsm.h create mode 100644 src/lib/rc/sbus.h diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index afde16ed3950..aed077fe5502 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -46,9 +46,16 @@ #include #include +#include "dsm.h" #include -#include "px4io.h" +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + #include +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + #include +#endif #define DSM_FRAME_SIZE 16 /** + */ + +#pragma once + +#include + +__BEGIN_DECLS + +__EXPORT int dsm_init(const char *device); +__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); +__EXPORT void dsm_bind(uint16_t cmd, int pulses); + +__END_DECLS diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 11f336a286cf..39642f0c1e2a 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -43,15 +43,9 @@ #include #include -#include - +#include "sbus.h" #include -#define DEBUG -#include "px4io.h" -#include "protocol.h" -#include "debug.h" - #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 #define SBUS_FLAGS_BYTE 23 @@ -77,8 +71,6 @@ #define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) #define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f)) -static int sbus_fd = -1; - static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; static hrt_abstime last_txframe_time = 0; @@ -93,11 +85,9 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_ bool *sbus_frame_drop, uint16_t max_channels); int -sbus_init(const char *device) +sbus_init(const char *device, bool singlewire) { - if (sbus_fd < 0) { - sbus_fd = open(device, O_RDWR | O_NONBLOCK); - } + int sbus_fd = open(device, O_RDWR | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -108,21 +98,24 @@ sbus_init(const char *device) t.c_cflag |= (CSTOPB | PARENB); tcsetattr(sbus_fd, TCSANOW, &t); + if (singlewire) { + /* only defined in configs capable of IOCTL */ + #ifdef SBUS_SERIAL_PORT + ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + #endif + } + /* initialise the decoder */ partial_frame_count = 0; last_rx_time = hrt_absolute_time(); - debug("S.Bus: ready"); - - } else { - debug("S.Bus: open failed"); } return sbus_fd; } void -sbus1_output(uint16_t *values, uint16_t num_values) +sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values) { uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; @@ -161,14 +154,14 @@ sbus1_output(uint16_t *values, uint16_t num_values) } } void -sbus2_output(uint16_t *values, uint16_t num_values) +sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values) { char b = 'B'; write(sbus_fd, &b, 1); } bool -sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) +sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -331,7 +324,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool } /* decode switch channels if data fields are wide enough */ - if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) { + if (max_values > 17 && chancount > 15) { chancount = 18; /* channel 17 (index 16) */ diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h new file mode 100644 index 000000000000..afe7a66e5212 --- /dev/null +++ b/src/lib/rc/sbus.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 sbus.h + * + * RC protocol definition for S.BUS + * + * @author Lorenz Meier + */ + +#pragma once + +#include +#include + +__BEGIN_DECLS + +__EXPORT int sbus_init(const char *device, bool singlewire); +__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_channels); +__EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); +__EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); + +__END_DECLS diff --git a/src/lib/rc/sumd.h b/src/lib/rc/sumd.h index f4793edfc8ea..cae9e5daa052 100644 --- a/src/lib/rc/sumd.h +++ b/src/lib/rc/sumd.h @@ -101,8 +101,9 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value); __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count); */ -int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, - uint16_t *channels, uint16_t max_chan_count); +__EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, + uint16_t *channels, uint16_t max_chan_count); + __END_DECLS From 296af4603b2fac4f48256e86fb6b983ba5dc44f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:14:53 +0100 Subject: [PATCH 192/293] Update IOv1 --- src/drivers/boards/px4io-v1/board_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index 59c17431c7e0..0d029bd0ac53 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -65,6 +65,8 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10) + /* Safety switch button *************************************************************/ #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) From b4fd3f73bd254fea4a35d859c990d215c2daa9a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:15:06 +0100 Subject: [PATCH 193/293] Update IOv2 config --- src/drivers/boards/px4io-v2/board_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index e9b17e7dc7d4..d97f3b203ce1 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -79,6 +79,8 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) #define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10) + /* Safety switch button *******************************************************/ #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) From 1c4f9c46fa27cd0f2a07098d120e785be43ea829 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 08:36:33 +1100 Subject: [PATCH 194/293] px4iofirmware: added build depencency on rc lib --- src/modules/px4iofirmware/module.mk | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 34c231174fb4..6794b20573e8 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -1,11 +1,11 @@ SRCS = adc.c \ controls.c \ - dsm.c \ px4io.c \ registers.c \ safety.c \ - sbus.c \ + ../../lib/rc/sbus.c \ + ../../lib/rc/dsm.c \ ../systemlib/up_cxxinitialize.c \ ../systemlib/perf_counter.c \ mixer.cpp \ @@ -27,4 +27,3 @@ endif SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) include $(SELF_DIR)../systemlib/mixer/multi_tables.mk - \ No newline at end of file From 3c43dbf5106033c7c0bad87dd2f8546f5892ca21 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:15:59 +0100 Subject: [PATCH 195/293] FMU driver: Add S.BUS and DSM FDs, not active yet --- src/drivers/px4fmu/fmu.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index eee184a4bb3f..46edb069596c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -139,6 +139,8 @@ class PX4FMU : public device::CDev actuator_armed_s _armed; unsigned _num_outputs; int _class_instance; + int _sbus_fd; + int _dsm_fd; volatile bool _task_should_exit; bool _servo_armed; @@ -275,6 +277,8 @@ PX4FMU::PX4FMU() : _num_outputs(0), _class_instance(0), _task_should_exit(false), + _sbus_fd(-1), + _dsm_fd(-1), _servo_armed(false), _pwm_on(false), _mixers(nullptr), @@ -303,7 +307,22 @@ PX4FMU::PX4FMU() : memset(_controls, 0, sizeof(_controls)); memset(_poll_fds, 0, sizeof(_poll_fds)); - _debug_enabled = true; +#ifdef HRT_PPM_CHANNEL + // rc input, published to ORB + memset(&_rc_in, 0, sizeof(_rc_in)); + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; +#endif + +#ifdef SBUS_SERIAL_PORT + _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); +#endif + +#ifdef DSM_SERIAL_PORT + _dsm_fd = dsm_init(DSM_SERIAL_PORT); +#endif + + /* only enable this during development */ + _debug_enabled = false; } PX4FMU::~PX4FMU() From a976b179e373161eb51f816ea040cb1ec97a8d50 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 08:41:58 +1100 Subject: [PATCH 196/293] rc: update to upstream version --- src/lib/rc/CMakeLists.txt | 44 ++++++++++ src/lib/rc/dsm.c | 52 ++++++++---- src/lib/rc/sbus.c | 7 +- src/lib/rc/sbus.h | 3 +- src/lib/rc/sumd.c | 170 ++++++++++++++++++++++++-------------- src/lib/rc/sumd.h | 5 +- 6 files changed, 195 insertions(+), 86 deletions(-) create mode 100644 src/lib/rc/CMakeLists.txt diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt new file mode 100644 index 000000000000..dd253a3f0ab7 --- /dev/null +++ b/src/lib/rc/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__rc + COMPILE_FLAGS + -Os + SRCS + st24.c + sumd.c + sbus.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index aed077fe5502..c068cdae0ca9 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -50,11 +50,11 @@ #include #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 - #include +#include #endif #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 - #include +#include #endif #define DSM_FRAME_SIZE 16 /**> shift) & 0xf; @@ -139,18 +140,21 @@ dsm_guess_format(bool reset) unsigned channel, value; /* if the channel decodes, remember the assigned number */ - if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) + if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) { cs10 |= (1 << channel); + } - if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) + if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) { cs11 |= (1 << channel); + } /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ } /* wait until we have seen plenty of frames - 5 should normally be enough */ - if (samples++ < 5) + if (samples++ < 5) { return; + } /* * Iterate the set of sensible sniffed channel sets and see whether @@ -177,11 +181,13 @@ dsm_guess_format(bool reset) for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) { - if (cs10 == masks[i]) + if (cs10 == masks[i]) { votes10++; + } - if (cs11 == masks[i]) + if (cs11 == masks[i]) { votes11++; + } } if ((votes11 == 1) && (votes10 == 0)) { @@ -217,8 +223,9 @@ dsm_init(const char *device) POWER_SPEKTRUM(true); #endif - if (dsm_fd < 0) + if (dsm_fd < 0) { dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + } if (dsm_fd >= 0) { @@ -261,8 +268,9 @@ dsm_bind(uint16_t cmd, int pulses) #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #else - if (dsm_fd < 0) + if (dsm_fd < 0) { return; + } switch (cmd) { @@ -302,6 +310,7 @@ dsm_bind(uint16_t cmd, int pulses) up_udelay(120); stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, true); } + break; case dsm_bind_reinit_uart: @@ -311,6 +320,7 @@ dsm_bind(uint16_t cmd, int pulses) break; } + #endif } @@ -334,8 +344,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * If we have lost signal for at least a second, reset the * format guessing heuristic. */ - if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { dsm_guess_format(true); + } /* we have received something we think is a dsm_frame */ dsm_last_frame_time = frame_time; @@ -363,20 +374,24 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; - if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) + if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) { continue; + } /* ignore channels out of range */ - if (channel >= PX4IO_RC_INPUT_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) { continue; + } /* update the decoded channel count */ - if (channel >= *num_values) + if (channel >= *num_values) { *num_values = channel + 1; + } /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ - if (dsm_channel_shift == 10) + if (dsm_channel_shift == 10) { value *= 2; + } /* * Spektrum scaling is special. There are these basic considerations @@ -426,8 +441,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * lines, so if we get a channel count of 13, we'll return 12 (the last * data index that is stable). */ - if (*num_values == 13) + if (*num_values == 13) { *num_values = 12; + } if (dsm_channel_shift == 11) { /* Set the 11-bit data indicator */ @@ -487,6 +503,7 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by /* if the read failed for any reason, just give up here */ if (ret < 1) { return false; + } else { *n_bytes = ret; *bytes = &dsm_frame[dsm_partial_frame_count]; @@ -502,8 +519,9 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by /* * If we don't have a full dsm frame, return */ - if (dsm_partial_frame_count < DSM_FRAME_SIZE) + if (dsm_partial_frame_count < DSM_FRAME_SIZE) { return false; + } /* * Great, it looks like we might have a dsm frame. Go ahead and diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 39642f0c1e2a..1ba1ff298e75 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -100,9 +100,9 @@ sbus_init(const char *device, bool singlewire) if (singlewire) { /* only defined in configs capable of IOCTL */ - #ifdef SBUS_SERIAL_PORT +#ifdef SBUS_SERIAL_PORT ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); - #endif +#endif } /* initialise the decoder */ @@ -161,7 +161,8 @@ sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values) } bool -sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) +sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_channels) { ssize_t ret; hrt_abstime now; diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index afe7a66e5212..1404a9eabd20 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -47,7 +47,8 @@ __BEGIN_DECLS __EXPORT int sbus_init(const char *device, bool singlewire); -__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, +__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, + bool *sbus_frame_drop, uint16_t max_channels); __EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); __EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.c index cea7790ec188..1d99f1d43392 100644 --- a/src/lib/rc/sumd.c +++ b/src/lib/rc/sumd.c @@ -95,9 +95,11 @@ uint16_t sumd_crc16(uint16_t crc, uint8_t value) { int i; crc ^= (uint16_t)value << 8; + for (i = 0; i < 8; i++) { crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); } + return crc; } @@ -108,15 +110,17 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value) } int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, - uint16_t max_chan_count) + uint16_t max_chan_count) { int ret = 1; + switch (_decode_state) { case SUMD_DECODE_STATE_UNSYNCED: - if(_debug) - printf( " SUMD_DECODE_STATE_UNSYNCED \n") ; - + if (_debug) { + printf(" SUMD_DECODE_STATE_UNSYNCED \n") ; + } + if (byte == SUMD_HEADER_ID) { _rxpacket.header = byte; _sumd = true; @@ -127,9 +131,11 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe _crc16 = sumd_crc16(_crc16, byte); _crc8 = sumd_crc8(_crc8, byte); _decode_state = SUMD_DECODE_STATE_GOT_HEADER; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ; + } + } else { ret = 3; } @@ -139,17 +145,23 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_HEADER: if (byte == SUMD_ID_SUMD || byte == SUMD_ID_SUMH) { _rxpacket.status = byte; + if (byte == SUMD_ID_SUMH) { _sumd = false; } + if (_sumd) { _crc16 = sumd_crc16(_crc16, byte); + } else { - _crc8 = sumd_crc8(_crc8, byte); + _crc8 = sumd_crc8(_crc8, byte); } + _decode_state = SUMD_DECODE_STATE_GOT_STATE; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ; + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ; + } } else { _decode_state = SUMD_DECODE_STATE_UNSYNCED; @@ -160,50 +172,65 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_STATE: if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) { _rxpacket.length = byte; + if (_sumd) { _crc16 = sumd_crc16(_crc16, byte); + } else { - _crc8 = sumd_crc8(_crc8, byte); + _crc8 = sumd_crc8(_crc8, byte); } + _rxlen++; _decode_state = SUMD_DECODE_STATE_GOT_LEN; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ; + } + } else { _decode_state = SUMD_DECODE_STATE_UNSYNCED; } + break; case SUMD_DECODE_STATE_GOT_LEN: _rxpacket.sumd_data[_rxlen] = byte; + if (_sumd) { _crc16 = sumd_crc16(_crc16, byte); + } else { - _crc8 = sumd_crc8(_crc8, byte); + _crc8 = sumd_crc8(_crc8, byte); } + _rxlen++; - - if (_rxlen <= ((_rxpacket.length*2) )) { - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen-2, byte) ; + + if (_rxlen <= ((_rxpacket.length * 2))) { + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen - 2, byte) ; + } } else { _decode_state = SUMD_DECODE_STATE_GOT_DATA; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ; + } + } break; case SUMD_DECODE_STATE_GOT_DATA: _rxpacket.crc16_high = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ; + } + if (_sumd) { _decode_state = SUMD_DECODE_STATE_GOT_CRC; + } else { _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_1; } @@ -212,91 +239,108 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_CRC16_BYTE_1: _rxpacket.crc16_low = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ; + } + _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_2; break; case SUMD_DECODE_STATE_GOT_CRC16_BYTE_2: _rxpacket.telemetry = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ; + } + _decode_state = SUMD_DECODE_STATE_GOT_CRC; break; - + case SUMD_DECODE_STATE_GOT_CRC: if (_sumd) { _rxpacket.crc16_low = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; - - if (_crc16 == (uint16_t)(_rxpacket.crc16_high<<8)+_rxpacket.crc16_low) { + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; + } + + if (_crc16 == (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low) { _crcOK = true; } + } else { _rxpacket.crc8 = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ; + } + if (_crc8 == _rxpacket.crc8) { _crcOK = true; } } - + if (_crcOK) { - if(_debug) - printf( " CRC - OK \n") ; + if (_debug) { + printf(" CRC - OK \n") ; + } if (_sumd) { - if(_debug) - printf( " Got valid SUMD Packet\n") ; - + if (_debug) { + printf(" Got valid SUMD Packet\n") ; + } + } else { - if(_debug) - printf( " Got valid SUMH Packet\n") ; - + if (_debug) { + printf(" Got valid SUMH Packet\n") ; + } + + } + + if (_debug) { + printf(" RXLEN: %d [Chans: %d] \n\n", _rxlen - 1, (_rxlen - 1) / 2) ; } - - if(_debug) - printf( " RXLEN: %d [Chans: %d] \n\n", _rxlen-1, (_rxlen-1)/2) ; ret = 0; unsigned i; uint8_t _cnt = *rx_count + 1; *rx_count = _cnt; - + *rssi = 100; /* received Channels */ if ((uint16_t)_rxpacket.length > max_chan_count) { _rxpacket.length = (uint8_t) max_chan_count; } + *channel_count = (uint16_t)_rxpacket.length; /* decode the actual packet */ /* reorder first 4 channels */ /* ch1 = roll -> sumd = ch2 */ - channels[0] = (uint16_t)((_rxpacket.sumd_data[1*2+1]<<8) | _rxpacket.sumd_data[1*2+2])>>3; + channels[0] = (uint16_t)((_rxpacket.sumd_data[1 * 2 + 1] << 8) | _rxpacket.sumd_data[1 * 2 + 2]) >> 3; /* ch2 = pitch -> sumd = ch2 */ - channels[1] = (uint16_t)((_rxpacket.sumd_data[2*2+1]<<8) | _rxpacket.sumd_data[2*2+2])>>3; + channels[1] = (uint16_t)((_rxpacket.sumd_data[2 * 2 + 1] << 8) | _rxpacket.sumd_data[2 * 2 + 2]) >> 3; /* ch3 = throttle -> sumd = ch2 */ - channels[2] = (uint16_t)((_rxpacket.sumd_data[0*2+1]<<8) | _rxpacket.sumd_data[0*2+2])>>3; + channels[2] = (uint16_t)((_rxpacket.sumd_data[0 * 2 + 1] << 8) | _rxpacket.sumd_data[0 * 2 + 2]) >> 3; /* ch4 = yaw -> sumd = ch2 */ - channels[3] = (uint16_t)((_rxpacket.sumd_data[3*2+1]<<8) | _rxpacket.sumd_data[3*2+2])>>3; + channels[3] = (uint16_t)((_rxpacket.sumd_data[3 * 2 + 1] << 8) | _rxpacket.sumd_data[3 * 2 + 2]) >> 3; /* we start at channel 5(index 4) */ unsigned chan_index = 4; for (i = 4; i < _rxpacket.length; i++) { - if(_debug) - printf( "ch[%d] : %x %x [ %x %d ]\n", i+1, _rxpacket.sumd_data[i*2+1], _rxpacket.sumd_data[i*2+2], ((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3, ((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3); - - channels[chan_index] = (uint16_t)((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3; + if (_debug) { + printf("ch[%d] : %x %x [ %x %d ]\n", i + 1, _rxpacket.sumd_data[i * 2 + 1], _rxpacket.sumd_data[i * 2 + 2], + ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3, + ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3); + } + + channels[chan_index] = (uint16_t)((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3; /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ //channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET; @@ -306,8 +350,10 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe } else { /* decoding failed */ ret = 4; - if(_debug) - printf( " CRC - fail \n") ; + + if (_debug) { + printf(" CRC - fail \n") ; + } } diff --git a/src/lib/rc/sumd.h b/src/lib/rc/sumd.h index cae9e5daa052..03d736418689 100644 --- a/src/lib/rc/sumd.h +++ b/src/lib/rc/sumd.h @@ -60,7 +60,7 @@ typedef struct { uint8_t header; ///< 0xA8 for a valid packet uint8_t status; ///< 0x01 valid and live SUMD data frame / 0x00 = SUMH / 0x81 = Failsafe uint8_t length; ///< Channels - uint8_t sumd_data[SUMD_MAX_CHANNELS*2]; ///< ChannelData (High Byte/ Low Byte) + uint8_t sumd_data[SUMD_MAX_CHANNELS * 2]; ///< ChannelData (High Byte/ Low Byte) uint8_t crc16_high; ///< High Byte of 16 Bit CRC uint8_t crc16_low; ///< Low Byte of 16 Bit CRC uint8_t telemetry; ///< Telemetry request @@ -102,8 +102,7 @@ __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_ uint16_t *channels, uint16_t max_chan_count); */ __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, - uint16_t *channels, uint16_t max_chan_count); + uint16_t *channels, uint16_t max_chan_count); - __END_DECLS From df821ba0a880c82c9e8d042f328d114653369b87 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 08:43:50 +1100 Subject: [PATCH 197/293] px4fmu: update to version from upstream --- src/drivers/px4fmu/CMakeLists.txt | 45 ++ src/drivers/px4fmu/fmu.cpp | 672 ++++++++++++++++-------------- 2 files changed, 411 insertions(+), 306 deletions(-) create mode 100644 src/drivers/px4fmu/CMakeLists.txt diff --git a/src/drivers/px4fmu/CMakeLists.txt b/src/drivers/px4fmu/CMakeLists.txt new file mode 100644 index 000000000000..492aad593914 --- /dev/null +++ b/src/drivers/px4fmu/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__px4fmu + MAIN fmu + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + fmu.cpp + px4fmu_params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 46edb069596c..9e1ea99861f0 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -90,6 +90,7 @@ */ #define CONTROL_INPUT_DROP_LIMIT_MS 20 +#define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev { @@ -132,17 +133,18 @@ class PX4FMU : public device::CDev unsigned _pwm_alt_rate; uint32_t _pwm_alt_rate_channels; unsigned _current_update_rate; - int _task; + struct work_s _work; int _armed_sub; int _param_sub; + struct rc_input_values _rc_in; + orb_advert_t _to_input_rc; orb_advert_t _outputs_pub; - actuator_armed_s _armed; unsigned _num_outputs; int _class_instance; int _sbus_fd; int _dsm_fd; - volatile bool _task_should_exit; + volatile bool _initialized; bool _servo_armed; bool _pwm_on; @@ -153,11 +155,11 @@ class PX4FMU : public device::CDev int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; - int _actuator_output_topic_instance; pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; - pwm_limit_t _pwm_limit; + static pwm_limit_t _pwm_limit; + static actuator_armed_s _armed; uint16_t _failsafe_pwm[_max_actuators]; uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; @@ -166,8 +168,13 @@ class PX4FMU : public device::CDev unsigned _num_failsafe_set; unsigned _num_disarmed_set; - static void task_main_trampoline(int argc, char *argv[]); - void task_main(); + static bool arm_nothrottle() { return (_armed.prearmed && !_armed.armed); } + + static void cycle_trampoline(void *arg); + + void cycle(); + void work_start(); + void work_stop(); static int control_callback(uintptr_t handle, uint8_t control_group, @@ -177,6 +184,7 @@ class PX4FMU : public device::CDev int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); void update_pwm_rev_mask(); + void publish_pwm_outputs(uint16_t *values, size_t numvalues); struct GPIOConfig { uint32_t input; @@ -196,8 +204,8 @@ class PX4FMU : public device::CDev int gpio_ioctl(file *filp, int cmd, unsigned long arg); /* do not allow to copy due to ptr data members */ - PX4FMU(const PX4FMU&); - PX4FMU operator=(const PX4FMU&); + PX4FMU(const PX4FMU &); + PX4FMU operator=(const PX4FMU &); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -253,7 +261,9 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { #endif }; -const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); +const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); +pwm_limit_t PX4FMU::_pwm_limit; +actuator_armed_s PX4FMU::_armed = {}; namespace { @@ -263,31 +273,30 @@ PX4FMU *g_fmu; } // namespace PX4FMU::PX4FMU() : - CDev("fmuservo", PX4FMU_DEVICE_PATH), + CDev("fmu", PX4FMU_DEVICE_PATH), _mode(MODE_NONE), _pwm_default_rate(50), _pwm_alt_rate(50), _pwm_alt_rate_channels(0), _current_update_rate(0), - _task(-1), + _work{}, _armed_sub(-1), _param_sub(-1), + _rc_in{}, + _to_input_rc(nullptr), _outputs_pub(nullptr), - _armed{}, _num_outputs(0), _class_instance(0), - _task_should_exit(false), _sbus_fd(-1), _dsm_fd(-1), + _initialized(false), _servo_armed(false), _pwm_on(false), _mixers(nullptr), _groups_required(0), _groups_subscribed(0), - _control_subs{-1}, - _actuator_output_topic_instance(-1), + _control_subs{ -1}, _poll_fds_num(0), - _pwm_limit{}, _failsafe_pwm{0}, _disarmed_pwm{0}, _reverse_pwm_mask(0), @@ -327,23 +336,18 @@ PX4FMU::PX4FMU() : PX4FMU::~PX4FMU() { - if (_task != -1) { + if (_initialized) { /* tell the task we want it to go away */ - _task_should_exit = true; + work_stop(); - unsigned i = 10; + int i = 10; do { /* wait 50ms - it should wake every 100ms or so worst-case */ usleep(50000); + i--; - /* if we have given up, kill it */ - if (--i == 0) { - task_delete(_task); - break; - } - - } while (_task != -1); + } while (_initialized && i > 0); } /* clean up the alternate device node */ @@ -357,48 +361,29 @@ PX4FMU::init() { int ret; - ASSERT(_task == -1); + ASSERT(!_initialized); /* do regular cdev init */ ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { - log("default PWM output device"); + /* lets not be too verbose */ } else if (_class_instance < 0) { - log("FAILED registering class device"); + warnx("FAILED registering class device"); } - /* reset GPIOs */ - gpio_reset(); - - /* start the IO interface task */ - _task = px4_task_spawn_cmd("fmuservo", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1600, - (main_t)&PX4FMU::task_main_trampoline, - nullptr); - - if (_task < 0) { - debug("task start failed: %d", errno); - return -errno; - } + work_start(); return OK; } -void -PX4FMU::task_main_trampoline(int argc, char *argv[]) -{ - g_fmu->task_main(); -} - int PX4FMU::set_mode(Mode mode) { @@ -411,7 +396,7 @@ PX4FMU::set_mode(Mode mode) */ switch (mode) { case MODE_2PWM: // v1 multi-port with flow control lines as PWM - debug("MODE_2PWM"); + DEVICE_DEBUG("MODE_2PWM"); /* default output rates */ _pwm_default_rate = 50; @@ -424,8 +409,8 @@ PX4FMU::set_mode(Mode mode) break; - case MODE_4PWM: // v1 multi-port as 4 PWM outs - debug("MODE_4PWM"); + case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs + DEVICE_DEBUG("MODE_4PWM"); /* default output rates */ _pwm_default_rate = 50; @@ -439,7 +424,7 @@ PX4FMU::set_mode(Mode mode) break; case MODE_6PWM: // v2 PWMs as 6 PWM outs - debug("MODE_6PWM"); + DEVICE_DEBUG("MODE_6PWM"); /* default output rates */ _pwm_default_rate = 50; @@ -453,8 +438,9 @@ PX4FMU::set_mode(Mode mode) break; #ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: // AeroCore PWMs as 8 PWM outs - debug("MODE_8PWM"); + DEVICE_DEBUG("MODE_8PWM"); /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; @@ -467,7 +453,7 @@ PX4FMU::set_mode(Mode mode) #endif case MODE_NONE: - debug("MODE_NONE"); + DEVICE_DEBUG("MODE_NONE"); _pwm_default_rate = 10; /* artificially reduced output rate */ _pwm_alt_rate = 10; @@ -489,7 +475,7 @@ PX4FMU::set_mode(Mode mode) int PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) { - debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); + DEVICE_DEBUG("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); for (unsigned pass = 0; pass < 2; pass++) { for (unsigned group = 0; group < _max_actuators; group++) { @@ -497,8 +483,9 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate // get the channel mask for this rate group uint32_t mask = up_pwm_servo_get_rate_group(group); - if (mask == 0) + if (mask == 0) { continue; + } // all channels in the group must be either default or alt-rate uint32_t alt = rate_map & mask; @@ -561,13 +548,15 @@ PX4FMU::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; _poll_fds_num = 0; + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { - warnx("subscribe to actuator_controls_%d", i); + DEVICE_DEBUG("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); } + if (unsub_groups & (1 << i)) { - warnx("unsubscribe from actuator_controls_%d", i); + DEVICE_DEBUG("unsubscribe from actuator_controls_%d", i); ::close(_control_subs[i]); _control_subs[i] = -1; } @@ -601,231 +590,256 @@ PX4FMU::update_pwm_rev_mask() } void -PX4FMU::task_main() +PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) { - /* force a reset of the update rate */ - _current_update_rate = 0; + actuator_outputs_s outputs; + outputs.noutputs = numvalues; + outputs.timestamp = hrt_absolute_time(); - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - _param_sub = orb_subscribe(ORB_ID(parameter_update)); + for (size_t i = 0; i < _max_actuators; ++i) { + outputs.output[i] = i < numvalues ? (float)values[i] : 0; + } - /* advertise the mixed control outputs */ - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); + if (_outputs_pub == nullptr) { + int instance = -1; + _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT); -#ifdef HRT_PPM_CHANNEL - // rc input, published to ORB - struct rc_input_values rc_in; - orb_advert_t to_input_rc = 0; + } else { + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); + } +} - memset(&rc_in, 0, sizeof(rc_in)); - rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; -#endif - /* initialize PWM limit lib */ - pwm_limit_init(&_pwm_limit); +void +PX4FMU::work_start() +{ + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, 0); +} + +void +PX4FMU::cycle_trampoline(void *arg) +{ + PX4FMU *dev = reinterpret_cast(arg); + + dev->cycle(); +} + +void +PX4FMU::cycle() +{ + if (!_initialized) { + /* reset GPIOs */ + gpio_reset(); + + /* force a reset of the update rate */ + _current_update_rate = 0; + + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _param_sub = orb_subscribe(ORB_ID(parameter_update)); - update_pwm_rev_mask(); + /* initialize PWM limit lib */ + pwm_limit_init(&_pwm_limit); - /* loop until killed */ - while (!_task_should_exit) { - if (_groups_subscribed != _groups_required) { - subscribe(); - _groups_subscribed = _groups_required; - /* force setting update rate */ - _current_update_rate = 0; + update_pwm_rev_mask(); + + _initialized = true; + } + + + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + /* force setting update rate */ + _current_update_rate = 0; + } + + /* + * Adjust actuator topic update rate to keep up with + * the highest servo update rate configured. + * + * We always mix at max rate; some channels may update slower. + */ + unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; + + if (_current_update_rate != max_rate) { + _current_update_rate = max_rate; + int update_rate_in_ms = int(1000 / _current_update_rate); + + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { + update_rate_in_ms = 2; } - /* - * Adjust actuator topic update rate to keep up with - * the highest servo update rate configured. - * - * We always mix at max rate; some channels may update slower. - */ - unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; - - if (_current_update_rate != max_rate) { - _current_update_rate = max_rate; - int update_rate_in_ms = int(1000 / _current_update_rate); - - /* reject faster than 500 Hz updates */ - if (update_rate_in_ms < 2) { - update_rate_in_ms = 2; - } + /* reject slower than 10 Hz updates */ + if (update_rate_in_ms > 100) { + update_rate_in_ms = 100; + } - /* reject slower than 10 Hz updates */ - if (update_rate_in_ms > 100) { - update_rate_in_ms = 100; - } + DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms); - debug("adjusted actuator update interval to %ums", update_rate_in_ms); - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] > 0) { - orb_set_interval(_control_subs[i], update_rate_in_ms); - } + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); } - - // set to current max rate, even if we are actually checking slower/faster - _current_update_rate = max_rate; } - /* sleep waiting for data, stopping to check for PPM - * input at 50Hz */ - int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS); + // set to current max rate, even if we are actually checking slower/faster + _current_update_rate = max_rate; + } + + /* check if anything updated */ + int ret = ::poll(_poll_fds, _poll_fds_num, 0); - /* this would be bad... */ - if (ret < 0) { - log("poll error %d", errno); - continue; + /* this would be bad... */ + if (ret < 0) { + DEVICE_LOG("poll error %d", errno); - } else if (ret == 0) { - /* timeout: no control data, switch to failsafe values */ + } else if (ret == 0) { + /* timeout: no control data, switch to failsafe values */ // warnx("no PWM: failsafe"); - } else { + } else { - /* get controls for required topics */ - unsigned poll_id = 0; - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] > 0) { - if (_poll_fds[poll_id].revents & POLLIN) { - orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); - } - poll_id++; + /* get controls for required topics */ + unsigned poll_id = 0; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } - } - /* can we mix? */ - if (_mixers != nullptr) { + poll_id++; + } + } - unsigned num_outputs; + /* can we mix? */ + if (_mixers != nullptr) { - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; + size_t num_outputs; - case MODE_4PWM: - num_outputs = 4; - break; + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; - case MODE_6PWM: - num_outputs = 6; - break; + case MODE_4PWM: + num_outputs = 4; + break; - case MODE_8PWM: - num_outputs = 8; - break; - default: - num_outputs = 0; - break; - } + case MODE_6PWM: + num_outputs = 6; + break; - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN and INF */ - if ((i >= outputs.noutputs) || - !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; - } - } + case MODE_8PWM: + num_outputs = 8; + break; - uint16_t pwm_limited[num_outputs]; + default: + num_outputs = 0; + break; + } - /* the PWM limit call takes care of out of band errors and constrains */ - pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + /* do mixing */ + float outputs[_max_actuators]; + num_outputs = _mixers->mix(outputs, num_outputs, NULL); - /* output to the servos */ - for (unsigned i = 0; i < num_outputs; i++) { - up_pwm_servo_set(i, pwm_limited[i]); + /* disable unused ports by setting their output to NaN */ + for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) { + if (i >= num_outputs) { + outputs[i] = NAN_VALUE; } + } - /* publish mixed control outputs */ - if (_outputs_pub != nullptr) { - _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT); - } else { + uint16_t pwm_limited[_max_actuators]; - orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); - } + /* the PWM limit call takes care of out of band errors, NaN and constrains */ + pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, + outputs, pwm_limited, &_pwm_limit); + + /* output to the servos */ + for (size_t i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); } + + publish_pwm_outputs(pwm_limited, num_outputs); } + } - /* check arming state */ - bool updated = false; - orb_check(_armed_sub, &updated); + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + 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 */ + bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown; - if (_servo_armed != set_armed) - _servo_armed = set_armed; + if (_servo_armed != set_armed) { + _servo_armed = set_armed; + } - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (_armed.armed || _num_disarmed_set > 0); + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (set_armed || _num_disarmed_set > 0); - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - up_pwm_servo_arm(pwm_on); - } + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); } + } - orb_check(_param_sub, &updated); - if (updated) { - parameter_update_s pupdate; - orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + orb_check(_param_sub, &updated); - update_pwm_rev_mask(); - } + if (updated) { + parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + + update_pwm_rev_mask(); + } #ifdef HRT_PPM_CHANNEL - // see if we have new PPM input data - if (ppm_last_valid_decode != rc_in.timestamp_last_signal) { - // we have a new PPM frame. Publish it. - rc_in.channel_count = ppm_decoded_channels; + // see if we have new PPM input data + if (ppm_last_valid_decode != _rc_in.timestamp_last_signal) { + // we have a new PPM frame. Publish it. + _rc_in.channel_count = ppm_decoded_channels; - if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { - rc_in.channel_count = RC_INPUT_MAX_CHANNELS; - } + if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } - for (uint8_t i = 0; i < rc_in.channel_count; i++) { - rc_in.values[i] = ppm_buffer[i]; - } + for (uint8_t i = 0; i < _rc_in.channel_count; i++) { + _rc_in.values[i] = ppm_buffer[i]; + } - rc_in.timestamp_publication = ppm_last_valid_decode; - rc_in.timestamp_last_signal = ppm_last_valid_decode; + _rc_in.timestamp_publication = ppm_last_valid_decode; + _rc_in.timestamp_last_signal = ppm_last_valid_decode; - rc_in.rc_ppm_frame_length = ppm_frame_length; - rc_in.rssi = RC_INPUT_RSSI_MAX; - rc_in.rc_failsafe = false; - rc_in.rc_lost = false; - rc_in.rc_lost_frame_count = 0; - rc_in.rc_total_frame_count = 0; + _rc_in.rc_ppm_frame_length = ppm_frame_length; + _rc_in.rssi = RC_INPUT_RSSI_MAX; + _rc_in.rc_failsafe = false; + _rc_in.rc_lost = false; + _rc_in.rc_lost_frame_count = 0; + _rc_in.rc_total_frame_count = 0; - /* lazily advertise on first publication */ - if (to_input_rc == 0) { - to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in); + /* lazily advertise on first publication */ + if (_to_input_rc == nullptr) { + _to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in); - } else { - orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); - } + } else { + orb_publish(ORB_ID(input_rc), _to_input_rc, &_rc_in); } + } #endif + work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000)); +} - } +void PX4FMU::work_stop() +{ + work_cancel(HPWORK, &_work); for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { @@ -833,19 +847,19 @@ PX4FMU::task_main() _control_subs[i] = -1; } } + ::close(_armed_sub); ::close(_param_sub); /* make sure servos are off */ up_pwm_servo_deinit(); - log("stopping"); + DEVICE_LOG("stopping"); /* note - someone else is responsible for restoring the GPIO config */ /* tell the dtor that we are exiting */ - _task = -1; - _exit(0); + _initialized = false; } int @@ -857,6 +871,35 @@ PX4FMU::control_callback(uintptr_t handle, const actuator_controls_s *controls = (actuator_controls_s *)handle; input = controls[control_group].control[control_index]; + + /* limit control input */ + if (input > 1.0f) { + input = 1.0f; + + } else if (input < -1.0f) { + input = -1.0f; + } + + /* motor spinup phase - lock throttle to zero */ + if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* limit the throttle output to zero during motor spinup, + * as the motors cannot follow any demand yet + */ + input = 0.0f; + } + } + + /* throttle not arming - mark throttle input as invalid */ + if (arm_nothrottle()) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* set the throttle to an invalid value */ + input = NAN_VALUE; + } + } + return 0; } @@ -865,14 +908,12 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - // XXX disabled, confusing users - //debug("ioctl 0x%04x 0x%08x", cmd, arg); - /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); - if (ret != -ENOTTY) + if (ret != -ENOTTY) { return ret; + } /* if we are in valid PWM mode, try it as a PWM ioctl as well */ switch (_mode) { @@ -886,13 +927,14 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) break; default: - debug("not in a PWM mode"); + DEVICE_DEBUG("not in a PWM mode"); break; } /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) + if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); + } return ret; } @@ -970,8 +1012,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) _num_failsafe_set = 0; for (unsigned i = 0; i < _max_actuators; i++) { - if (_failsafe_pwm[i] > 0) + if (_failsafe_pwm[i] > 0) { _num_failsafe_set++; + } } break; @@ -1018,8 +1061,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) _num_disarmed_set = 0; for (unsigned i = 0; i < _max_actuators; i++) { - if (_disarmed_pwm[i] > 0) + if (_disarmed_pwm[i] > 0) { _num_disarmed_set++; + } } break; @@ -1113,12 +1157,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) } #ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_SET(7): case PWM_SERVO_SET(6): if (_mode < MODE_8PWM) { ret = -EINVAL; break; } + #endif case PWM_SERVO_SET(5): @@ -1149,12 +1195,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; #ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_GET(7): case PWM_SERVO_GET(6): if (_mode < MODE_8PWM) { ret = -EINVAL; break; } + #endif case PWM_SERVO_GET(5): @@ -1195,6 +1243,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case MIXERIOCGETOUTPUTCOUNT: switch (_mode) { #ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: *(unsigned *)arg = 8; break; @@ -1220,25 +1269,25 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_COUNT: { - /* change the number of outputs that are enabled for - * PWM. This is used to change the split between GPIO - * and PWM under control of the flight config - * parameters. Note that this does not allow for - * changing a set of pins to be used for serial on - * FMUv1 - */ - switch (arg) { - case 0: - set_mode(MODE_NONE); - break; + /* change the number of outputs that are enabled for + * PWM. This is used to change the split between GPIO + * and PWM under control of the flight config + * parameters. Note that this does not allow for + * changing a set of pins to be used for serial on + * FMUv1 + */ + switch (arg) { + case 0: + set_mode(MODE_NONE); + break; - case 2: - set_mode(MODE_2PWM); - break; + case 2: + set_mode(MODE_2PWM); + break; - case 4: - set_mode(MODE_4PWM); - break; + case 4: + set_mode(MODE_4PWM); + break; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) @@ -1247,17 +1296,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) - case 8: - set_mode(MODE_8PWM); - break; + + case 8: + set_mode(MODE_8PWM); + break; #endif - default: - ret = -EINVAL; + default: + ret = -EINVAL; + break; + } + break; } - break; - } case MIXERIOCRESET: if (_mixers != nullptr) { @@ -1295,8 +1346,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - if (_mixers == nullptr) + if (_mixers == nullptr) { _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); + } if (_mixers == nullptr) { _groups_required = 0; @@ -1307,11 +1359,12 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - debug("mixer load failed with %d", ret); + DEVICE_DEBUG("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; _groups_required = 0; ret = -EINVAL; + } else { _mixers->groups_required(_groups_required); @@ -1342,15 +1395,19 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) uint16_t values[6]; #ifdef CONFIG_ARCH_BOARD_AEROCORE + if (count > 8) { // we have at most 8 outputs count = 8; } + #else + if (count > 6) { // we have at most 6 outputs count = 6; } + #endif // allow for misaligned values @@ -1435,6 +1492,10 @@ PX4FMU::sensor_reset(int ms) stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_configgpio(GPIO_SPI1_SCK); + stm32_configgpio(GPIO_SPI1_MISO); + stm32_configgpio(GPIO_SPI1_MOSI); + // // XXX bring up the EXTI pins again // stm32_configgpio(GPIO_GYRO_DRDY); // stm32_configgpio(GPIO_MAG_DRDY); @@ -1603,10 +1664,9 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) gpios |= 3; /* flip the buffer to output mode if required */ - if (GPIO_SET_OUTPUT == function || - GPIO_SET_OUTPUT_LOW == function || - GPIO_SET_OUTPUT_HIGH == function) + if (GPIO_SET_OUTPUT == function) { stm32_gpiowrite(GPIO_GPIO_DIR, 1); + } } #endif @@ -1623,17 +1683,10 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) stm32_configgpio(_gpio_tab[i].output); break; - case GPIO_SET_OUTPUT_LOW: - stm32_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_SET)) | GPIO_OUTPUT_CLEAR); - break; - - case GPIO_SET_OUTPUT_HIGH: - stm32_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_CLEAR)) | GPIO_OUTPUT_SET); - break; - case GPIO_SET_ALT_1: - if (_gpio_tab[i].alt != 0) + if (_gpio_tab[i].alt != 0) { stm32_configgpio(_gpio_tab[i].alt); + } break; } @@ -1643,8 +1696,9 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* flip buffer to input mode if required */ - if ((GPIO_SET_INPUT == function) && (gpios & 3)) + if ((GPIO_SET_INPUT == function) && (gpios & 3)) { stm32_gpiowrite(GPIO_GPIO_DIR, 0); + } #endif } @@ -1655,8 +1709,9 @@ PX4FMU::gpio_write(uint32_t gpios, int function) int value = (function == GPIO_SET) ? 1 : 0; for (unsigned i = 0; i < _ngpio; i++) - if (gpios & (1 << i)) + if (gpios & (1 << i)) { stm32_gpiowrite(_gpio_tab[i].output, value); + } } uint32_t @@ -1665,8 +1720,9 @@ PX4FMU::gpio_read(void) uint32_t bits = 0; for (unsigned i = 0; i < _ngpio; i++) - if (stm32_gpioread(_gpio_tab[i].input)) + if (stm32_gpioread(_gpio_tab[i].input)) { bits |= (1 << i); + } return bits; } @@ -1693,8 +1749,6 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) break; case GPIO_SET_OUTPUT: - case GPIO_SET_OUTPUT_LOW: - case GPIO_SET_OUTPUT_HIGH: case GPIO_SET_INPUT: case GPIO_SET_ALT_1: gpio_set_function(arg, cmd); @@ -1736,7 +1790,6 @@ enum PortMode { PORT_PWM_AND_SERIAL, PORT_PWM_AND_GPIO, PORT_PWM4, - PORT_PWM6, }; PortMode g_port_mode; @@ -1778,11 +1831,6 @@ fmu_new_mode(PortMode new_mode) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; break; - - case PORT_PWM6: - /* select 6-pin PWM mode */ - servo_mode = PX4FMU::MODE_6PWM; - break; #endif /* mixed modes supported on v1 board only */ @@ -1816,8 +1864,9 @@ fmu_new_mode(PortMode new_mode) } /* adjust GPIO config for serial mode(s) */ - if (gpio_bits != 0) + if (gpio_bits != 0) { g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + } /* (re)set the PWM output mode */ g_fmu->set_mode(servo_mode); @@ -1916,10 +1965,11 @@ test(void) fd = open(PX4FMU_DEVICE_PATH, O_RDWR); - if (fd < 0) + if (fd < 0) { errx(1, "open fail"); + } - if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); + if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) { err(1, "servo arm failed"); } if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { err(1, "Unable to get servo count\n"); @@ -1937,8 +1987,9 @@ test(void) /* sweep all servos between 1000..2000 */ servo_position_t servos[servo_count]; - for (unsigned i = 0; i < servo_count; i++) + for (unsigned i = 0; i < servo_count; i++) { servos[i] = pwm_value; + } if (direction == 1) { // use ioctl interface for one direction @@ -1952,8 +2003,9 @@ test(void) // and use write interface for the other direction ret = write(fd, servos, sizeof(servos)); - if (ret != (int)sizeof(servos)) + if (ret != (int)sizeof(servos)) { err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + } } if (direction > 0) { @@ -1977,11 +2029,13 @@ test(void) for (unsigned i = 0; i < servo_count; i++) { servo_position_t value; - if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) + if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) { err(1, "error reading PWM servo %d", i); + } - if (value != servos[i]) + if (value != servos[i]) { errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); + } } /* Check if user wants to quit */ @@ -2007,8 +2061,9 @@ test(void) void fake(int argc, char *argv[]) { - if (argc < 5) + if (argc < 5) { errx(1, "fmu fake (values -100 .. 100)"); + } actuator_controls_s ac; @@ -2022,8 +2077,9 @@ fake(int argc, char *argv[]) orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - if (handle == nullptr) + if (handle == nullptr) { errx(1, "advertise failed"); + } actuator_armed_s aa; @@ -2032,8 +2088,9 @@ fake(int argc, char *argv[]) handle = orb_advertise(ORB_ID(actuator_armed), &aa); - if (handle == nullptr) + if (handle == nullptr) { errx(1, "advertise failed 2"); + } exit(0); } @@ -2063,8 +2120,9 @@ fmu_main(int argc, char *argv[]) } - if (fmu_start() != OK) + if (fmu_start() != OK) { errx(1, "failed to start the FMU driver"); + } /* * Mode switches. @@ -2079,8 +2137,6 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm4")) { new_mode = PORT_PWM4; - } else if (!strcmp(verb, "mode_pwm6")) { - new_mode = PORT_PWM6; #endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) @@ -2102,19 +2158,22 @@ fmu_main(int argc, char *argv[]) if (new_mode != PORT_MODE_UNSET) { /* yes but it's the same mode */ - if (new_mode == g_port_mode) + if (new_mode == g_port_mode) { return OK; + } /* switch modes */ int ret = fmu_new_mode(new_mode); exit(ret == OK ? 0 : 1); } - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { test(); + } - if (!strcmp(verb, "fake")) + if (!strcmp(verb, "fake")) { fake(argc - 1, argv + 1); + } if (!strcmp(verb, "sensor_reset")) { if (argc > 2) { @@ -2153,6 +2212,7 @@ fmu_main(int argc, char *argv[]) } exit(0); + } else { warnx("i2c cmd args: "); } From 5d74e17423bd088879cebdfdf77e3d1b1eeb1273 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:01:46 +1100 Subject: [PATCH 198/293] msg: update from upstream --- msg/actuator_armed.msg | 1 + msg/actuator_controls.msg | 9 +++ msg/airspeed.msg | 3 +- msg/att_pos_mocap.msg | 10 +++ msg/camera_trigger.msg | 4 + msg/control_state.msg | 19 +++++ msg/estimator_status.msg | 1 + msg/geofence_result.msg | 7 ++ msg/hil_sensor.msg | 100 ++++++++++++++++++++++++ msg/home_position.msg | 5 ++ msg/input_rc.msg | 22 ++++++ msg/manual_control_setpoint.msg | 1 + msg/mission.msg | 3 + msg/mission_result.msg | 12 +++ msg/multirotor_motor_limits.msg | 4 + msg/navigation_capabilities.msg | 6 ++ msg/output_pwm.msg | 3 + msg/rc_channels.msg | 7 +- msg/rc_parameter_map.msg | 11 +++ msg/safety.msg | 3 + msg/satellite_info.msg | 9 +++ msg/sensor_accel.msg | 17 ++++ msg/sensor_baro.msg | 5 ++ msg/sensor_combined.msg | 124 ++++++++++++------------------ msg/sensor_gyro.msg | 17 ++++ msg/sensor_mag.msg | 12 +++ msg/tecs_status.msg | 11 ++- msg/telemetry_status.msg | 19 +++++ msg/uavcan_parameter_request.msg | 8 ++ msg/uavcan_parameter_value.msg | 8 ++ msg/vehicle_attitude.msg | 3 + msg/vehicle_attitude_setpoint.msg | 4 + msg/vehicle_command.msg | 5 +- msg/vehicle_status.msg | 20 ++++- msg/vehicle_vicon_position.msg | 10 --- msg/vtol_vehicle_status.msg | 1 + 36 files changed, 407 insertions(+), 97 deletions(-) create mode 100644 msg/att_pos_mocap.msg create mode 100644 msg/camera_trigger.msg create mode 100644 msg/control_state.msg create mode 100644 msg/hil_sensor.msg create mode 100644 msg/input_rc.msg create mode 100644 msg/mission.msg create mode 100644 msg/mission_result.msg create mode 100644 msg/multirotor_motor_limits.msg create mode 100644 msg/navigation_capabilities.msg create mode 100644 msg/output_pwm.msg create mode 100644 msg/rc_parameter_map.msg create mode 100644 msg/safety.msg create mode 100644 msg/satellite_info.msg create mode 100644 msg/sensor_accel.msg create mode 100644 msg/sensor_baro.msg create mode 100644 msg/sensor_gyro.msg create mode 100644 msg/sensor_mag.msg create mode 100644 msg/telemetry_status.msg create mode 100644 msg/uavcan_parameter_request.msg create mode 100644 msg/uavcan_parameter_value.msg delete mode 100644 msg/vehicle_vicon_position.msg diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg index c8098724e86a..f307e366ae59 100644 --- a/msg/actuator_armed.msg +++ b/msg/actuator_armed.msg @@ -1,6 +1,7 @@ uint64 timestamp # Microseconds since system boot bool armed # Set to true if system is armed +bool prearmed # Set to true if the actuator safety is disabled but motors are not 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) bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 414eb06ddb26..a6ebda6aae06 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -1,5 +1,14 @@ uint8 NUM_ACTUATOR_CONTROLS = 8 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint8 INDEX_ROLL = 0 +uint8 INDEX_PITCH = 1 +uint8 INDEX_YAW = 2 +uint8 INDEX_THROTTLE = 3 +uint8 INDEX_FLAPS = 4 +uint8 INDEX_SPOILERS = 5 +uint8 INDEX_AIRBRAKES = 6 +uint8 INDEX_LANDING_GEAR = 7 +uint8 GROUP_INDEX_ATTITUDE = 0 uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control diff --git a/msg/airspeed.msg b/msg/airspeed.msg index 8d6af2138d5e..525bfd7f88ed 100644 --- a/msg/airspeed.msg +++ b/msg/airspeed.msg @@ -1,4 +1,5 @@ uint64 timestamp # microseconds since system boot, needed to integrate float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown -float32 true_airspeed_m_s # true airspeed in meters per second, -1 if unknown +float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown +float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown diff --git a/msg/att_pos_mocap.msg b/msg/att_pos_mocap.msg new file mode 100644 index 000000000000..52bc04b5aabf --- /dev/null +++ b/msg/att_pos_mocap.msg @@ -0,0 +1,10 @@ +uint32 id # ID of the estimator, commonly the component ID of the incoming message + +uint64 timestamp_boot # time of this estimate, in microseconds since system start +uint64 timestamp_computer # timestamp provided by the companion computer, in us + +float32[4] q # Estimated attitude as quaternion + +float32 x # X position in meters in NED earth-fixed frame +float32 y # Y position in meters in NED earth-fixed frame +float32 z # Z position in meters in NED earth-fixed frame (negative altitude) diff --git a/msg/camera_trigger.msg b/msg/camera_trigger.msg new file mode 100644 index 000000000000..b4dcfe8ef39b --- /dev/null +++ b/msg/camera_trigger.msg @@ -0,0 +1,4 @@ + +uint64 timestamp # Timestamp when camera was triggered +uint32 seq # Image sequence + diff --git a/msg/control_state.msg b/msg/control_state.msg new file mode 100644 index 000000000000..4f130353d3ab --- /dev/null +++ b/msg/control_state.msg @@ -0,0 +1,19 @@ +# This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */ +uint64 timestamp # in microseconds since system start +float32 x_acc # X acceleration in body frame +float32 y_acc # Y acceleration in body frame +float32 z_acc # Z acceleration in body frame +float32 x_vel # X velocity in body frame +float32 y_vel # Y velocity in body frame +float32 z_vel # Z velocity in body frame +float32 x_pos # X position in local frame +float32 y_pos # Y position in local frame +float32 z_pos # z position in local frame +float32 airspeed # Airspeed, estimated +float32[3] vel_variance # Variance in body velocity estimate +float32[3] pos_variance # Variance in local position estimate +float32[4] q # Attitude Quaternion +float32 roll_rate # Roll body angular rate (rad/s, x forward/y right/z down) +float32 pitch_rate # Pitch body angular rate (rad/s, x forward/y right/z down) +float32 yaw_rate # Yaw body angular rate (rad/s, x forward/y right/z down) +float32 horz_acc_mag # Magnitude of the horizontal acceleration diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 92e5303a6ad4..ccbd2386db2b 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -4,3 +4,4 @@ float32 n_states # Number of states effectively used uint8 nan_flags # Bitmask to indicate NaN states uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt) uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt) +float32[28] covariances # Diagonal Elements of Covariance Matrix diff --git a/msg/geofence_result.msg b/msg/geofence_result.msg index 7fc21c2af804..165d92b98965 100644 --- a/msg/geofence_result.msg +++ b/msg/geofence_result.msg @@ -1 +1,8 @@ +uint8 GF_ACTION_NONE = 0 # no action on geofence violation +uint8 GF_ACTION_WARN = 1 # critical mavlink message +uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER +uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL +uint8 GF_ACTION_TERMINATE = 4 # flight termination + bool geofence_violated # true if the geofence is violated +uint8 geofence_action # action to take when geofence is violated \ No newline at end of file diff --git a/msg/hil_sensor.msg b/msg/hil_sensor.msg new file mode 100644 index 000000000000..9317722db4b1 --- /dev/null +++ b/msg/hil_sensor.msg @@ -0,0 +1,100 @@ +# Definition of the hil_sensor uORB topic. + +int32 MAGNETOMETER_MODE_NORMAL = 0 +int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1 +int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 + +# Sensor readings in raw and SI-unit form. +# +# These values are read from the sensors. Raw values are in sensor-specific units, +# the scaled values are in SI-units, as visible from the ending of the variable +# or the comments. The use of the SI fields is in general advised, as these fields +# are scaled and offset-compensated where possible and do not change with board +# revisions and sensor updates. +# +# Actual data, this is specific to the type of data which is stored in this struct +# A line containing L0GME will be added by the Python logging code generator to the logged dataset. +# +# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only + +uint64 timestamp # Timestamp in microseconds since boot, from gyro +# +int16[3] gyro_raw # Raw sensor values of angular velocity +float32[3] gyro_rad_s # Angular velocity in radian per seconds +uint32 gyro_errcount # Error counter for gyro 0 +float32 gyro_temp # Temperature of gyro 0 + +int16[3] accelerometer_raw # Raw acceleration in NED body frame +float32[3] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 +int16 accelerometer_mode # Accelerometer measurement mode +float32 accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 +uint64 accelerometer_timestamp # Accelerometer timestamp +uint32 accelerometer_errcount # Error counter for accel 0 +float32 accelerometer_temp # Temperature of accel 0 + +int16[3] magnetometer_raw # Raw magnetic field in NED body frame +float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss +int16 magnetometer_mode # Magnetometer measurement mode +float32 magnetometer_range_ga # measurement range in Gauss +float32 magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor +uint64 magnetometer_timestamp # Magnetometer timestamp +uint32 magnetometer_errcount # Error counter for mag 0 +float32 magnetometer_temp # Temperature of mag 0 + +int16[3] gyro1_raw # Raw sensor values of angular velocity +float32[3] gyro1_rad_s # Angular velocity in radian per seconds +uint64 gyro1_timestamp # Gyro timestamp +uint32 gyro1_errcount # Error counter for gyro 1 +float32 gyro1_temp # Temperature of gyro 1 + +int16[3] accelerometer1_raw # Raw acceleration in NED body frame +float32[3] accelerometer1_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer1_timestamp # Accelerometer timestamp +uint32 accelerometer1_errcount # Error counter for accel 1 +float32 accelerometer1_temp # Temperature of accel 1 + +int16[3] magnetometer1_raw # Raw magnetic field in NED body frame +float32[3] magnetometer1_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer1_timestamp # Magnetometer timestamp +uint32 magnetometer1_errcount # Error counter for mag 1 +float32 magnetometer1_temp # Temperature of mag 1 + +int16[3] gyro2_raw # Raw sensor values of angular velocity +float32[3] gyro2_rad_s # Angular velocity in radian per seconds +uint64 gyro2_timestamp # Gyro timestamp +uint32 gyro2_errcount # Error counter for gyro 1 +float32 gyro2_temp # Temperature of gyro 1 + +int16[3] accelerometer2_raw # Raw acceleration in NED body frame +float32[3] accelerometer2_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer2_timestamp # Accelerometer timestamp +uint32 accelerometer2_errcount # Error counter for accel 2 +float32 accelerometer2_temp # Temperature of accel 2 + +int16[3] magnetometer2_raw # Raw magnetic field in NED body frame +float32[3] magnetometer2_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer2_timestamp # Magnetometer timestamp +uint32 magnetometer2_errcount # Error counter for mag 2 +float32 magnetometer2_temp # Temperature of mag 2 + +float32 baro_pres_mbar # Barometric pressure, already temp. comp. +float32 baro_alt_meter # Altitude, already temp. comp. +float32 baro_temp_celcius # Temperature in degrees celsius +uint64 baro_timestamp # Barometer timestamp + +float32 baro1_pres_mbar # Barometric pressure, already temp. comp. +float32 baro1_alt_meter # Altitude, already temp. comp. +float32 baro1_temp_celcius # Temperature in degrees celsius +uint64 baro1_timestamp # Barometer timestamp + +float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1 +uint16[10] adc_mapping # Channel indices of each of these values +float32 mcu_temp_celcius # Internal temperature measurement of MCU + +float32 differential_pressure_pa # Airspeed sensor differential pressure +uint64 differential_pressure_timestamp # Last measurement timestamp +float32 differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading + +float32 differential_pressure1_pa # Airspeed sensor differential pressure +uint64 differential_pressure1_timestamp # Last measurement timestamp +float32 differential_pressure1_filtered_pa # Low pass filtered airspeed sensor differential pressure reading diff --git a/msg/home_position.msg b/msg/home_position.msg index d8aff3f6342d..7135c07b18ad 100644 --- a/msg/home_position.msg +++ b/msg/home_position.msg @@ -8,3 +8,8 @@ float32 alt # Altitude in meters (AMSL) float32 x # X coordinate in meters float32 y # Y coordinate in meters float32 z # Z coordinate in meters + +float32 yaw # Yaw angle in radians +float32 direction_x # Takeoff direction in NED X +float32 direction_y # Takeoff direction in NED Y +float32 direction_z # Takeoff direction in NED Z diff --git a/msg/input_rc.msg b/msg/input_rc.msg new file mode 100644 index 000000000000..8e15a9a186d2 --- /dev/null +++ b/msg/input_rc.msg @@ -0,0 +1,22 @@ +uint8 RC_INPUT_SOURCE_UNKNOWN = 0 +uint8 RC_INPUT_SOURCE_PX4FMU_PPM = 1 +uint8 RC_INPUT_SOURCE_PX4IO_PPM = 2 +uint8 RC_INPUT_SOURCE_PX4IO_SPEKTRUM = 3 +uint8 RC_INPUT_SOURCE_PX4IO_SBUS = 4 +uint8 RC_INPUT_SOURCE_PX4IO_ST24 = 5 +uint8 RC_INPUT_SOURCE_MAVLINK = 6 +uint8 RC_INPUT_SOURCE_QURT = 7 + +uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. + +uint64 timestamp_publication # publication time +uint64 timestamp_last_signal # last valid reception time +uint32 channel_count # number of channels actually being seen +int32 rssi # receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception +bool rc_failsafe # explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly. +bool rc_lost # RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. +uint16 rc_lost_frame_count # Number of lost RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality. +uint16 rc_total_frame_count # Number of total RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality. +uint16 rc_ppm_frame_length # Length of a single PPM frame. Zero for non-PPM systems +uint8 input_source # Input source +uint16[18] values # measured pulse widths for each of the supported channels diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index d3cfb078be88..dd1f7d0b5e51 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -38,6 +38,7 @@ float32 aux5 # default function: payload drop uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL +uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO diff --git a/msg/mission.msg b/msg/mission.msg new file mode 100644 index 000000000000..ac135a4e090b --- /dev/null +++ b/msg/mission.msg @@ -0,0 +1,3 @@ +int32 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint32 count # count of the missions stored in the dataman +int32 current_seq # default -1, start at the one changed latest diff --git a/msg/mission_result.msg b/msg/mission_result.msg new file mode 100644 index 000000000000..ac4d32f559a4 --- /dev/null +++ b/msg/mission_result.msg @@ -0,0 +1,12 @@ +uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified +uint32 seq_reached # Sequence of the mission item which has been reached +uint32 seq_current # Sequence of the current mission item +bool valid # true if mission is valid +bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings +bool reached # true if mission has been reached +bool finished # true if mission has been completed +bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode +bool flight_termination # true if the navigator demands a flight termination from the commander app +bool item_do_jump_changed # true if the number of do jumps remaining has changed +uint32 item_changed_index # indicate which item has changed +uint32 item_do_jump_remaining # set to the number of do jumps remaining for that item diff --git a/msg/multirotor_motor_limits.msg b/msg/multirotor_motor_limits.msg new file mode 100644 index 000000000000..2e03afc040b7 --- /dev/null +++ b/msg/multirotor_motor_limits.msg @@ -0,0 +1,4 @@ +uint8 lower_limit # at least one actuator command has saturated on the lower limit +uint8 upper_limit # at least one actuator command has saturated on the upper limit +uint8 yaw # yaw limit reached +uint8 reserved # reserved diff --git a/msg/navigation_capabilities.msg b/msg/navigation_capabilities.msg new file mode 100644 index 000000000000..6d12aaaed911 --- /dev/null +++ b/msg/navigation_capabilities.msg @@ -0,0 +1,6 @@ +uint64 timestamp # timestamp this topic was emitted +float32 turn_distance # the optimal distance to a waypoint to switch to the next +float32 landing_horizontal_slope_displacement +float32 landing_slope_angle_rad +float32 landing_flare_length +bool abort_landing diff --git a/msg/output_pwm.msg b/msg/output_pwm.msg new file mode 100644 index 000000000000..1418a43e6e57 --- /dev/null +++ b/msg/output_pwm.msg @@ -0,0 +1,3 @@ +uint8 PWM_OUTPUT_MAX_CHANNELS = 16 +uint16[16] values +uint32 channel_count diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 0fa5ed2fc4d9..b2e08d864a44 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,4 +1,4 @@ -int32 RC_CHANNELS_FUNCTION_MAX=19 +int32 RC_CHANNELS_FUNCTION_MAX=20 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 uint8 RC_CHANNELS_FUNCTION_ROLL=1 uint8 RC_CHANNELS_FUNCTION_PITCH=2 @@ -18,10 +18,11 @@ uint8 RC_CHANNELS_FUNCTION_AUX_5=15 uint8 RC_CHANNELS_FUNCTION_PARAM_1=16 uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 +uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19 uint64 timestamp # Timestamp in microseconds since boot time uint64 timestamp_last_valid # Timestamp of last valid RC signal -float32[19] channels # Scaled to -1..1 (throttle: 0..1) +float32[20] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[19] function # Functions mapping +int8[20] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout diff --git a/msg/rc_parameter_map.msg b/msg/rc_parameter_map.msg new file mode 100644 index 000000000000..b2dd1843ab4e --- /dev/null +++ b/msg/rc_parameter_map.msg @@ -0,0 +1,11 @@ +uint8 RC_PARAM_MAP_NCHAN = 3 # This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h +uint8 PARAM_ID_LEN = 16 # corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + +uint64 timestamp # time at which the map was updated +bool[3] valid #true for RC-Param channels which are mapped to a param +int32[3] param_index # corresponding param index, this field is ignored if set to -1, in this case param_id will be used +char[51] param_id # MAP_NCHAN * (ID_LEN + 1) chars, corresponding param id, null terminated +float32[3] scale # scale to map the RC input [-1, 1] to a parameter value +float32[3] value0 # inital value around which the parameter value is changed +float32[3] value_min # minimal parameter value +float32[3] value_max # minimal parameter value diff --git a/msg/safety.msg b/msg/safety.msg new file mode 100644 index 000000000000..be0fadad1e28 --- /dev/null +++ b/msg/safety.msg @@ -0,0 +1,3 @@ +uint64 timestamp +bool safety_switch_available # Set to true if a safety switch is connected +bool safety_off # Set to true if safety is off diff --git a/msg/satellite_info.msg b/msg/satellite_info.msg new file mode 100644 index 000000000000..24cb03facca1 --- /dev/null +++ b/msg/satellite_info.msg @@ -0,0 +1,9 @@ +uint8 SAT_INFO_MAX_SATELLITES = 20 + +uint64 timestamp # Timestamp of satellite info +uint8 count # Number of satellites in satellite info +uint8[20] svid # Space vehicle ID [1..255], see scheme below +uint8[20] used # 0: Satellite not used, 1: used for navigation +uint8[20] elevation # Elevation (0: right on top of receiver, 90: on the horizon) of satellite +uint8[20] azimuth # Direction of satellite, 0: 0 deg, 255: 360 deg. +uint8[20] snr # dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg new file mode 100644 index 000000000000..2eca55746507 --- /dev/null +++ b/msg/sensor_accel.msg @@ -0,0 +1,17 @@ +uint64 timestamp +uint64 integral_dt # integration time +uint64 error_count +float32 x # acceleration in the NED X board axis in m/s^2 +float32 y # acceleration in the NED Y board axis in m/s^2 +float32 z # acceleration in the NED Z board axis in m/s^2 +float32 x_integral # velocity in the NED X board axis in m/s over the integration time frame +float32 y_integral # velocity in the NED Y board axis in m/s over the integration time frame +float32 z_integral # velocity in the NED Z board axis in m/s over the integration time frame +float32 temperature # temperature in degrees celsius +float32 range_m_s2 # range in m/s^2 (+- this value) +float32 scaling + +int16 x_raw +int16 y_raw +int16 z_raw +int16 temperature_raw diff --git a/msg/sensor_baro.msg b/msg/sensor_baro.msg new file mode 100644 index 000000000000..315d5a56ece2 --- /dev/null +++ b/msg/sensor_baro.msg @@ -0,0 +1,5 @@ +float32 pressure +float32 altitude +float32 temperature +uint64 timestamp +uint64 error_count diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index c5e427e19406..ec58e6f923f7 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -4,6 +4,14 @@ int32 MAGNETOMETER_MODE_NORMAL = 0 int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1 int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 +uint32 SENSOR_PRIO_MIN = 0 +uint32 SENSOR_PRIO_VERY_LOW = 25 +uint32 SENSOR_PRIO_LOW = 50 +uint32 SENSOR_PRIO_DEFAULT = 75 +uint32 SENSOR_PRIO_HIGH = 100 +uint32 SENSOR_PRIO_VERY_HIGH = 125 +uint32 SENSOR_PRIO_MAX = 255 + # Sensor readings in raw and SI-unit form. # # These values are read from the sensors. Raw values are in sensor-specific units, @@ -18,83 +26,49 @@ int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 # NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only uint64 timestamp # Timestamp in microseconds since boot, from gyro -# -int16[3] gyro_raw # Raw sensor values of angular velocity -float32[3] gyro_rad_s # Angular velocity in radian per seconds -uint32 gyro_errcount # Error counter for gyro 0 -float32 gyro_temp # Temperature of gyro 0 - -int16[3] accelerometer_raw # Raw acceleration in NED body frame -float32[3] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 -int16 accelerometer_mode # Accelerometer measurement mode -float32 accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 -uint64 accelerometer_timestamp # Accelerometer timestamp -uint32 accelerometer_errcount # Error counter for accel 0 -float32 accelerometer_temp # Temperature of accel 0 - -int16[3] magnetometer_raw # Raw magnetic field in NED body frame -float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss -int16 magnetometer_mode # Magnetometer measurement mode -float32 magnetometer_range_ga # measurement range in Gauss -float32 magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor -uint64 magnetometer_timestamp # Magnetometer timestamp -uint32 magnetometer_errcount # Error counter for mag 0 -float32 magnetometer_temp # Temperature of mag 0 - -int16[3] gyro1_raw # Raw sensor values of angular velocity -float32[3] gyro1_rad_s # Angular velocity in radian per seconds -uint64 gyro1_timestamp # Gyro timestamp -uint32 gyro1_errcount # Error counter for gyro 1 -float32 gyro1_temp # Temperature of gyro 1 - -int16[3] accelerometer1_raw # Raw acceleration in NED body frame -float32[3] accelerometer1_m_s2 # Acceleration in NED body frame, in m/s^2 -uint64 accelerometer1_timestamp # Accelerometer timestamp -uint32 accelerometer1_errcount # Error counter for accel 1 -float32 accelerometer1_temp # Temperature of accel 1 - -int16[3] magnetometer1_raw # Raw magnetic field in NED body frame -float32[3] magnetometer1_ga # Magnetic field in NED body frame, in Gauss -uint64 magnetometer1_timestamp # Magnetometer timestamp -uint32 magnetometer1_errcount # Error counter for mag 1 -float32 magnetometer1_temp # Temperature of mag 1 - -int16[3] gyro2_raw # Raw sensor values of angular velocity -float32[3] gyro2_rad_s # Angular velocity in radian per seconds -uint64 gyro2_timestamp # Gyro timestamp -uint32 gyro2_errcount # Error counter for gyro 1 -float32 gyro2_temp # Temperature of gyro 1 - -int16[3] accelerometer2_raw # Raw acceleration in NED body frame -float32[3] accelerometer2_m_s2 # Acceleration in NED body frame, in m/s^2 -uint64 accelerometer2_timestamp # Accelerometer timestamp -uint32 accelerometer2_errcount # Error counter for accel 2 -float32 accelerometer2_temp # Temperature of accel 2 - -int16[3] magnetometer2_raw # Raw magnetic field in NED body frame -float32[3] magnetometer2_ga # Magnetic field in NED body frame, in Gauss -uint64 magnetometer2_timestamp # Magnetometer timestamp -uint32 magnetometer2_errcount # Error counter for mag 2 -float32 magnetometer2_temp # Temperature of mag 2 - -float32 baro_pres_mbar # Barometric pressure, already temp. comp. -float32 baro_alt_meter # Altitude, already temp. comp. -float32 baro_temp_celcius # Temperature in degrees celsius -uint64 baro_timestamp # Barometer timestamp - -float32 baro1_pres_mbar # Barometric pressure, already temp. comp. -float32 baro1_alt_meter # Altitude, already temp. comp. -float32 baro1_temp_celcius # Temperature in degrees celsius -uint64 baro1_timestamp # Barometer timestamp +uint64[3] gyro_timestamp # Gyro timestamps +int16[9] gyro_raw # Raw sensor values of angular velocity +float32[9] gyro_rad_s # Angular velocity in radian per seconds +uint32[3] gyro_priority # Sensor priority +float32[9] gyro_integral_rad # delta angle in radians +uint64[3] gyro_integral_dt # delta time for gyro integral in us +uint32[3] gyro_errcount # Error counter for gyro 0 +float32[3] gyro_temp # Temperature of gyro 0 + +int16[9] accelerometer_raw # Raw acceleration in NED body frame +float32[9] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 +float32[9] accelerometer_integral_m_s # velocity in NED body frame, in m/s^2 +uint64[3] accelerometer_integral_dt # delta time for accel integral in us +int16[3] accelerometer_mode # Accelerometer measurement mode +float32[3] accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 +uint64[3] accelerometer_timestamp # Accelerometer timestamp +uint32[3] accelerometer_priority # Sensor priority +uint32[3] accelerometer_errcount # Error counter for accel 0 +float32[3] accelerometer_temp # Temperature of accel 0 + +int16[9] magnetometer_raw # Raw magnetic field in NED body frame +float32[9] magnetometer_ga # Magnetic field in NED body frame, in Gauss +int16[3] magnetometer_mode # Magnetometer measurement mode +float32[3] magnetometer_range_ga # measurement range in Gauss +float32[3] magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor +uint64[3] magnetometer_timestamp # Magnetometer timestamp +uint32[3] magnetometer_priority # Sensor priority +uint32[3] magnetometer_errcount # Error counter for mag 0 +float32[3] magnetometer_temp # Temperature of mag 0 + +float32[3] baro_pres_mbar # Barometric pressure, already temp. comp. +float32[3] baro_alt_meter # Altitude, already temp. comp. +float32[3] baro_temp_celcius # Temperature in degrees celsius +uint64[3] baro_timestamp # Barometer timestamp +uint32[3] baro_priority # Sensor priority +uint32[3] baro_errcount # Error count in communication float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1 uint16[10] adc_mapping # Channel indices of each of these values float32 mcu_temp_celcius # Internal temperature measurement of MCU -float32 differential_pressure_pa # Airspeed sensor differential pressure -uint64 differential_pressure_timestamp # Last measurement timestamp -float32 differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading - -float32 differential_pressure1_pa # Airspeed sensor differential pressure -uint64 differential_pressure1_timestamp # Last measurement timestamp -float32 differential_pressure1_filtered_pa # Low pass filtered airspeed sensor differential pressure reading +float32[3] differential_pressure_pa # Airspeed sensor differential pressure +uint64[3] differential_pressure_timestamp # Last measurement timestamp +float32[3] differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading +uint32[3] differential_pressure_priority # Sensor priority +uint32[3] differential_pressure_errcount # Error count in communication diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg new file mode 100644 index 000000000000..65aa4a870676 --- /dev/null +++ b/msg/sensor_gyro.msg @@ -0,0 +1,17 @@ +uint64 timestamp +uint64 integral_dt # integration time +uint64 error_count +float32 x # angular velocity in the NED X board axis in rad/s +float32 y # angular velocity in the NED Y board axis in rad/s +float32 z # angular velocity in the NED Z board axis in rad/s +float32 x_integral # delta angle in the NED X board axis in rad/s in the integration time frame +float32 y_integral # delta angle in the NED Y board axis in rad/s in the integration time frame +float32 z_integral # delta angle in the NED Z board axis in rad/s in the integration time frame +float32 temperature # temperature in degrees celcius +float32 range_rad_s +float32 scaling + +int16 x_raw +int16 y_raw +int16 z_raw +int16 temperature_raw diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg new file mode 100644 index 000000000000..3ac97227ea85 --- /dev/null +++ b/msg/sensor_mag.msg @@ -0,0 +1,12 @@ +uint64 timestamp +uint64 error_count +float32 x +float32 y +float32 z +float32 range_ga +float32 scaling +float32 temperature + +int16 x_raw +int16 y_raw +int16 z_raw diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index ccac921e2b3f..4dcd16b2ca89 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -18,9 +18,12 @@ float32 airspeed_filtered float32 airspeedDerivativeSp float32 airspeedDerivative -float32 totalEnergyRateSp -float32 totalEnergyRate -float32 energyDistributionRateSp -float32 energyDistributionRate +float32 totalEnergyError +float32 energyDistributionError +float32 totalEnergyRateError +float32 energyDistributionRateError + +float32 throttle_integ +float32 pitch_integ uint8 mode diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg new file mode 100644 index 000000000000..cdcef931afc7 --- /dev/null +++ b/msg/telemetry_status.msg @@ -0,0 +1,19 @@ +uint8 TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0 +uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1 +uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2 +uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3 +uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4 + +uint64 timestamp +uint64 heartbeat_time # Time of last received heartbeat from remote system +uint64 telem_time # Time of last received telemetry status packet, 0 for none +uint8 type # type of the radio hardware +uint8 rssi # local signal strength +uint8 remote_rssi # remote signal strength +uint16 rxerrors # receive errors +uint16 fixed # count of error corrected packets +uint8 noise # background noise level +uint8 remote_noise # remote background noise level +uint8 txbuf # how full the tx buffer is as a percentage +uint8 system_id # system id of the remote system +uint8 component_id # component id of the remote system diff --git a/msg/uavcan_parameter_request.msg b/msg/uavcan_parameter_request.msg new file mode 100644 index 000000000000..9ced52bbaed6 --- /dev/null +++ b/msg/uavcan_parameter_request.msg @@ -0,0 +1,8 @@ +# UAVCAN-MAVLink parameter bridge request type +uint8 message_type # MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # -1 if the param_id field should be used as identifier +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/msg/uavcan_parameter_value.msg b/msg/uavcan_parameter_value.msg new file mode 100644 index 000000000000..091c5fd278bc --- /dev/null +++ b/msg/uavcan_parameter_value.msg @@ -0,0 +1,8 @@ +# UAVCAN-MAVLink parameter bridge response type +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # parameter index, if known +uint16 param_count # number of parameters exposed by the node +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index 63879149adbc..0ee90cd61da9 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -10,6 +10,9 @@ float32 yawspeed # Yaw body angular rate (rad/s, x forward/y right/z down) float32 rollacc # Roll angular accelration (rad/s^2, x forward/y right/z down) float32 pitchacc # Pitch angular acceleration (rad/s^2, x forward/y right/z down) float32 yawacc # Yaw angular acceleration (rad/s^2, x forward/y right/z down) +float32 rate_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels. +float32 accel_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels. +float32 mag_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels. float32[3] rate_offsets # Offsets of the body angular rates from zero float32[9] R # Rotation matrix, body to world, (Tait-Bryan, NED) float32[4] q # Quaternion (NED) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 7bbb670b31e9..43ea237b4796 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -21,3 +21,7 @@ float32 thrust # Thrust in Newton the power system should generate bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) + +bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) + +bool apply_flaps diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 391dc01aa052..3a4fb3a596d3 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -48,9 +48,12 @@ uint32 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutd uint32 VEHICLE_CMD_OVERRIDE_GOTO = 252 # Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| uint32 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| uint32 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm| -uint32 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +uint32 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system +uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment +uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started uint32 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | uint32 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 07484425b3f7..60ab34e90d2e 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -7,7 +7,9 @@ uint8 MAIN_STATE_AUTO_LOITER = 4 uint8 MAIN_STATE_AUTO_RTL = 5 uint8 MAIN_STATE_ACRO = 6 uint8 MAIN_STATE_OFFBOARD = 7 -uint8 MAIN_STATE_MAX = 8 +uint8 MAIN_STATE_STAB = 8 +uint8 MAIN_STATE_RATTITUDE = 9 +uint8 MAIN_STATE_MAX = 10 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -39,7 +41,9 @@ uint8 NAVIGATION_STATE_LAND = 11 # Land mode uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 -uint8 NAVIGATION_STATE_MAX = 15 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "flip") mode +uint8 NAVIGATION_STATE_MAX = 17 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 @@ -77,6 +81,13 @@ uint8 VEHICLE_TYPE_VTOL_HEXAROTOR = 21 # Vtol with six engines uint8 VEHICLE_TYPE_VTOL_OCTOROTOR = 22 # Vtol with eight engines uint8 VEHICLE_TYPE_ENUM_END = 23 +# VEHICLE_VTOL_STATE, should match 1:1 MAVLinks's MAV_VTOL_STATE +uint8 VEHICLE_VTOL_STATE_UNDEFINED = 0 +uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1 +uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2 +uint8 VEHICLE_VTOL_STATE_MC = 3 +uint8 VEHICLE_VTOL_STATE_FW = 4 + uint8 VEHICLE_BATTERY_WARNING_NONE = 0 # no battery low voltage warning active uint8 VEHICLE_BATTERY_WARNING_LOW = 1 # warning of low voltage uint8 VEHICLE_BATTERY_WARNING_CRITICAL = 2 # alerting of critical voltage @@ -104,14 +115,16 @@ uint32 component_id # subsystem / component id, inspired by MAVLink's componen bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter bool is_vtol # True if the system is VTOL capable bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode +bool in_transition_mode bool condition_battery_voltage_valid bool condition_system_in_air_restore # true if we can restore in mid air bool condition_system_sensors_initialized +bool condition_system_prearm_error_reported # true if errors have already been reported +bool condition_system_hotplug_timeout # true if the hotplug sensor search is over bool condition_system_returned_to_home bool condition_auto_mission_available bool condition_global_position_valid # set to true by the commander app if the quality of the position estimate is good enough to use it for navigation -bool condition_launch_position_valid # indicates a valid launch position bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) bool condition_local_position_valid bool condition_local_altitude_valid @@ -168,3 +181,4 @@ bool circuit_breaker_engaged_power_check bool circuit_breaker_engaged_airspd_check bool circuit_breaker_engaged_enginefailure_check bool circuit_breaker_engaged_gpsfailure_check +bool cb_usb diff --git a/msg/vehicle_vicon_position.msg b/msg/vehicle_vicon_position.msg deleted file mode 100644 index 1626d85383a2..000000000000 --- a/msg/vehicle_vicon_position.msg +++ /dev/null @@ -1,10 +0,0 @@ -uint64 timestamp # time of this estimate, in microseconds since system start -bool valid # true if position satisfies validity criteria of estimator - -float32 x # X position in meters in NED earth-fixed frame -float32 y # Y position in meters in NED earth-fixed frame -float32 z # Z position in meters in NED earth-fixed frame (negative altitude) -float32 roll -float32 pitch -float32 yaw -float32[4] q # Attitude as quaternion diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg index 5b2dc991c910..a1be5d5867b4 100644 --- a/msg/vtol_vehicle_status.msg +++ b/msg/vtol_vehicle_status.msg @@ -1,4 +1,5 @@ uint64 timestamp # Microseconds since system boot bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode +bool vtol_in_trans_mode bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode float32 airspeed_tot # Estimated airspeed over control surfaces From 56498acb881367e5391949dcb2c26776d152ebf5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:02:05 +1100 Subject: [PATCH 199/293] boards: update from upstream --- .../px4-stm32f4discovery/CMakeLists.txt | 42 +++++++++++++++++ .../px4-stm32f4discovery/board_config.h | 10 ++-- .../px4-stm32f4discovery/px4discovery_led.c | 15 +++--- src/drivers/boards/px4fmu-v1/CMakeLists.txt | 47 +++++++++++++++++++ src/drivers/boards/px4fmu-v1/board_config.h | 12 +++++ src/drivers/boards/px4fmu-v1/px4fmu_init.c | 36 +++++++------- src/drivers/boards/px4fmu-v1/px4fmu_led.c | 33 ++++++------- src/drivers/boards/px4fmu-v1/px4fmu_spi.c | 6 +-- src/drivers/boards/px4fmu-v2/CMakeLists.txt | 47 +++++++++++++++++++ src/drivers/boards/px4fmu-v2/board_config.h | 14 ++++++ src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 37 ++++++++------- src/drivers/boards/px4fmu-v2/px4fmu2_led.c | 15 +++--- src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 2 +- src/drivers/boards/px4fmu-v4/px4fmu_led.c | 2 +- src/drivers/boards/px4io-v1/CMakeLists.txt | 43 +++++++++++++++++ src/drivers/boards/px4io-v1/board_config.h | 12 ++--- .../px4io-v2/{px4iov2_init.c => px4io_init.c} | 0 ...{px4iov2_pwm_servo.c => px4io_pwm_servo.c} | 0 18 files changed, 292 insertions(+), 81 deletions(-) create mode 100644 src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt create mode 100644 src/drivers/boards/px4fmu-v1/CMakeLists.txt create mode 100644 src/drivers/boards/px4fmu-v2/CMakeLists.txt create mode 100644 src/drivers/boards/px4io-v1/CMakeLists.txt rename src/drivers/boards/px4io-v2/{px4iov2_init.c => px4io_init.c} (100%) rename src/drivers/boards/px4io-v2/{px4iov2_pwm_servo.c => px4io_pwm_servo.c} (100%) diff --git a/src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt b/src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt new file mode 100644 index 000000000000..e4af694e5d7a --- /dev/null +++ b/src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__px4-stm32f4discovery + SRCS + px4discovery_init.c + px4discovery_usb.c + px4discovery_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h index c4f469caa1f4..d06a3a317b62 100644 --- a/src/drivers/boards/px4-stm32f4discovery/board_config.h +++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ @@ -66,13 +66,13 @@ __BEGIN_DECLS #define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) #define GPIO_LED4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) /* SPI chip selects */ #define GPIO_SPI_CS_MEMS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c index 1f42843edf53..6a6c6eaa0d31 100644 --- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c @@ -69,8 +69,7 @@ __EXPORT void led_init() __EXPORT void led_on(int led) { - if (led == 1) - { + if (led == 1) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED1, false); } @@ -78,8 +77,7 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 1) - { + if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); } @@ -87,11 +85,12 @@ __EXPORT void led_off(int led) __EXPORT void led_toggle(int led) { - if (led == 1) - { - if (stm32_gpioread(GPIO_LED1)) + if (led == 1) { + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } } } diff --git a/src/drivers/boards/px4fmu-v1/CMakeLists.txt b/src/drivers/boards/px4fmu-v1/CMakeLists.txt new file mode 100644 index 000000000000..8807ed6730ee --- /dev/null +++ b/src/drivers/boards/px4fmu-v1/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__px4fmu-v1 + COMPILE_FLAGS + -Os + SRCS + px4fmu_can.c + px4fmu_init.c + px4fmu_pwm_servo.c + px4fmu_spi.c + px4fmu_usb.c + px4fmu_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 37bebcbfd8c8..5957ce6fc5dd 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -120,6 +120,18 @@ __BEGIN_DECLS #define PX4_I2C_OBDEV_PX4IO_BL 0x18 #define PX4_I2C_OBDEV_PX4IO 0x1a +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + */ +#define ADC_CHANNELS (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) + +// ADC defines to be used in sensors.cpp to read from a particular channel +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 + /* User GPIOs * * GPIO0-1 are the buffered high-power GPIOs. diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 2b2b5ccd7a58..dea76780141e 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -145,6 +145,7 @@ static struct spi_dev_s *spi3; #include +#if 0 #ifdef __cplusplus __EXPORT int matherr(struct __exception *e) { @@ -156,6 +157,7 @@ __EXPORT int matherr(struct exception *e) return 1; } #endif +#endif __EXPORT int nsh_archinitialize(void) { @@ -221,23 +223,23 @@ __EXPORT int nsh_archinitialize(void) * Keep the SPI2 init optional and conditionally initialize the ADC pins */ - #ifdef CONFIG_STM32_SPI2 - spi2 = up_spiinitialize(2); - /* Default SPI2 to 1MHz and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi2, 10000000); - SPI_SETBITS(spi2, 8); - SPI_SETMODE(spi2, SPIDEV_MODE3); - SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); - - message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); - #else - spi2 = NULL; - message("[boot] Enabling IN12/13 instead of SPI2\n"); - /* no SPI2, use pins for ADC */ - stm32_configgpio(GPIO_ADC1_IN12); - stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards - #endif +#ifdef CONFIG_STM32_SPI2 + spi2 = up_spiinitialize(2); + /* Default SPI2 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 10000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); + + message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); +#else + spi2 = NULL; + message("[boot] Enabling IN12/13 instead of SPI2\n"); + /* no SPI2, use pins for ADC */ + stm32_configgpio(GPIO_ADC1_IN12); + stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards +#endif /* Get the SPI port for the microSD slot */ diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c index 888e4f551b09..8a92cc6e3f52 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -70,13 +70,12 @@ __EXPORT void led_init(void) __EXPORT void led_on(int led) { - if (led == 0) - { + if (led == 0) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED1, false); } - if (led == 1) - { + + if (led == 1) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED2, false); } @@ -84,13 +83,12 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 0) - { + if (led == 0) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); } - if (led == 1) - { + + if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED2, true); } @@ -98,18 +96,21 @@ __EXPORT void led_off(int led) __EXPORT void led_toggle(int led) { - if (led == 0) - { - if (stm32_gpioread(GPIO_LED1)) + if (led == 0) { + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } } - if (led == 1) - { - if (stm32_gpioread(GPIO_LED2)) + + if (led == 1) { + if (stm32_gpioread(GPIO_LED2)) { stm32_gpiowrite(GPIO_LED2, false); - else + + } else { stm32_gpiowrite(GPIO_LED2, true); + } } } diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c index 8ef17c36ed74..e877a6dff4b7 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -67,7 +67,7 @@ * ************************************************************************************/ -__EXPORT void weak_function stm32_spiinitialize(void) +__EXPORT void stm32_spiinitialize(void) { stm32_configgpio(GPIO_SPI_CS_GYRO); stm32_configgpio(GPIO_SPI_CS_ACCEL); @@ -90,8 +90,8 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); break; diff --git a/src/drivers/boards/px4fmu-v2/CMakeLists.txt b/src/drivers/boards/px4fmu-v2/CMakeLists.txt new file mode 100644 index 000000000000..19b3f027544e --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__px4fmu-v2 + COMPILE_FLAGS + -Os + SRCS + px4fmu_can.c + px4fmu2_init.c + px4fmu_pwm_servo.c + px4fmu_spi.c + px4fmu_usb.c + px4fmu2_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 0332d3fecf07..ea60df9100b5 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -113,6 +113,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_RAMTRON 2 #define PX4_SPI_BUS_EXT 4 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ @@ -146,6 +147,19 @@ __BEGIN_DECLS #define PX4_I2C_OBDEV_LED 0x55 #define PX4_I2C_OBDEV_HMC5883 0x1e +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + */ +#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15) + +// ADC defines to be used in sensors.cpp to read from a particular channel +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 + /* User GPIOs * * GPIO0-5 are the PWM servo outputs. diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 698258fa45e0..116a6ff55671 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -118,12 +118,12 @@ __END_DECLS static GRAN_HANDLE dma_allocator; -/* - * The DMA heap size constrains the total number of things that can be +/* + * The DMA heap size constrains the total number of things that can be * ready to do DMA at a time. * * For example, FAT DMA depends on one sector-sized buffer per filesystem plus - * one sector-sized buffer per file. + * one sector-sized buffer per file. * * We use a fundamental alignment / granule size of 64B; this is sufficient * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). @@ -138,8 +138,10 @@ dma_alloc_init(void) sizeof(g_dma_heap), 7, /* 128B granule - must be > alignment (XXX bug?) */ 6); /* 64B alignment */ + if (dma_allocator == NULL) { message("[boot] DMA allocator setup FAILED"); + } else { g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); } @@ -210,17 +212,18 @@ static struct sdio_dev_s *sdio; #include -#ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) -{ - return 1; -} -#else -__EXPORT int matherr(struct exception *e) -{ - return 1; -} -#endif +/* TODO XXX commented this out to get cmake build working */ +/*#ifdef __cplusplus*/ +/*__EXPORT int matherr(struct __exception *e)*/ +/*{*/ +/*return 1;*/ +/*}*/ +/*#else*/ +/*__EXPORT int matherr(struct exception *e)*/ +/*{*/ +/*return 1;*/ +/*}*/ +/*#endif*/ __EXPORT int nsh_archinitialize(void) { @@ -325,10 +328,11 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false); SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false); - #ifdef CONFIG_MMCSD +#ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + if (!sdio) { message("[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); @@ -337,6 +341,7 @@ __EXPORT int nsh_archinitialize(void) /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + if (ret != OK) { message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; @@ -345,7 +350,7 @@ __EXPORT int nsh_archinitialize(void) /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ sdio_mediachange(sdio, true); - #endif +#endif return OK; } diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c index 74f77b3ef6e6..09edb2ba661b 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -69,8 +69,7 @@ __EXPORT void led_init() __EXPORT void led_on(int led) { - if (led == 1) - { + if (led == 1) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED1, false); } @@ -78,8 +77,7 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 1) - { + if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); } @@ -87,11 +85,12 @@ __EXPORT void led_off(int led) __EXPORT void led_toggle(int led) { - if (led == 1) - { - if (stm32_gpioread(GPIO_LED1)) + if (led == 1) { + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } } } diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 64b9926b8de1..a380e05bd5b8 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -67,7 +67,7 @@ * ************************************************************************************/ -__EXPORT void weak_function stm32_spiinitialize(void) +__EXPORT void stm32_spiinitialize(void) { #ifdef CONFIG_STM32_SPI1 stm32_configgpio(GPIO_SPI_CS_GYRO); diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_led.c b/src/drivers/boards/px4fmu-v4/px4fmu_led.c index 59ed10483e8f..ddb34e8a5648 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_led.c @@ -72,7 +72,7 @@ static uint32_t g_ledmap[] = { __EXPORT void led_init(void) { /* Configure LED GPIOs for output */ - for (size_t l = 0; l < (sizeof(g_ledmap)/sizeof(g_ledmap[0])); l++) { + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { stm32_configgpio(g_ledmap[l]); } } diff --git a/src/drivers/boards/px4io-v1/CMakeLists.txt b/src/drivers/boards/px4io-v1/CMakeLists.txt new file mode 100644 index 000000000000..0806a1e04910 --- /dev/null +++ b/src/drivers/boards/px4io-v1/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__px4io-v1 + COMPILE_FLAGS + -Os + SRCS + px4io_init.c + px4io_pwm_servo.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index 0d029bd0ac53..f83f18e50306 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -30,10 +30,10 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - + /** * @file board_config.h - * + * * PX4IO hardware definitions. */ @@ -59,11 +59,11 @@ /* LEDs */ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) #define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10) @@ -88,7 +88,7 @@ #define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) #define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) -/* +/* * High-resolution timer */ #define HRT_TIMER 1 /* use timer1 for the HRT */ diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4io_init.c similarity index 100% rename from src/drivers/boards/px4io-v2/px4iov2_init.c rename to src/drivers/boards/px4io-v2/px4io_init.c diff --git a/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c b/src/drivers/boards/px4io-v2/px4io_pwm_servo.c similarity index 100% rename from src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c rename to src/drivers/boards/px4io-v2/px4io_pwm_servo.c From 5d9545aca83c6974aea7f04aa1a1b3e5047428fe Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:02:25 +1100 Subject: [PATCH 200/293] device: update from upstream --- src/drivers/device/CMakeLists.txt | 68 ++++ src/drivers/device/cdev.cpp | 48 ++- src/drivers/device/device_nuttx.cpp | 44 +-- src/drivers/device/device_nuttx.h | 64 ++-- src/drivers/device/device_posix.cpp | 54 +--- src/drivers/device/i2c_nuttx.cpp | 41 ++- src/drivers/device/i2c_nuttx.h | 3 +- src/drivers/device/i2c_posix.cpp | 80 +++-- src/drivers/device/i2c_posix.h | 4 +- src/drivers/device/integrator.cpp | 128 ++++++++ src/drivers/device/integrator.h | 97 ++++++ src/drivers/device/module.mk | 4 +- src/drivers/device/ringbuffer.cpp | 35 +- src/drivers/device/ringbuffer.h | 7 +- src/drivers/device/sim.cpp | 4 +- src/drivers/device/sim.h | 10 +- src/drivers/device/spi.cpp | 24 +- src/drivers/device/spi.h | 8 +- src/drivers/device/vdev.cpp | 173 +++++++--- src/drivers/device/vdev.h | 105 +++--- src/drivers/device/vdev_posix.cpp | 476 +++++++++++++++------------- src/drivers/device/vfile.cpp | 2 +- src/drivers/device/vfile.h | 3 +- 23 files changed, 952 insertions(+), 530 deletions(-) create mode 100644 src/drivers/device/CMakeLists.txt create mode 100644 src/drivers/device/integrator.cpp create mode 100644 src/drivers/device/integrator.h diff --git a/src/drivers/device/CMakeLists.txt b/src/drivers/device/CMakeLists.txt new file mode 100644 index 000000000000..95df2d1855c5 --- /dev/null +++ b/src/drivers/device/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +set(SRCS) + +list(APPEND SRCS + ringbuffer.cpp + integrator.cpp +) + +if(${OS} STREQUAL "nuttx") + if (NOT ${BOARD} STREQUAL "sim") + list(APPEND SRCS + device_nuttx.cpp + cdev.cpp + i2c_nuttx.cpp + pio.cpp + spi.cpp + ) + endif() +else() + list(APPEND SRCS + device_posix.cpp + vdev.cpp + vfile.cpp + vdev_posix.cpp + i2c_posix.cpp + sim.cpp + ) +endif() + +px4_add_module( + MODULE drivers__device + SRCS ${SRCS} + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 3bc05b0c7348..c43ab1353e50 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -100,14 +100,16 @@ CDev::CDev(const char *name, _registered(false), _open_count(0) { - for (unsigned i = 0; i < _max_pollwaiters; i++) + for (unsigned i = 0; i < _max_pollwaiters; i++) { _pollset[i] = nullptr; + } } CDev::~CDev() { - if (_registered) + if (_registered) { unregister_driver(_devname); + } } int @@ -124,13 +126,16 @@ CDev::register_class_devname(const char *class_devname) char name[32]; snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); ret = register_driver(name, &fops, 0666, (void *)this); - if (ret == OK) break; + + if (ret == OK) { break; } + class_instance++; } if (class_instance == 4) { return ret; } + return class_instance; } @@ -148,15 +153,17 @@ CDev::init() // base class init first int ret = Device::init(); - if (ret != OK) + if (ret != OK) { goto out; + } // now register the driver if (_devname != nullptr) { ret = register_driver(_devname, &fops, 0666, (void *)this); - if (ret != OK) + if (ret != OK) { goto out; + } _registered = true; } @@ -182,8 +189,9 @@ CDev::open(file_t *filp) /* first-open callback may decline the open */ ret = open_first(filp); - if (ret != OK) + if (ret != OK) { _open_count--; + } } unlock(); @@ -209,8 +217,9 @@ CDev::close(file_t *filp) _open_count--; /* callback cannot decline the close */ - if (_open_count == 0) + if (_open_count == 0) { ret = close_last(filp); + } } else { ret = -EBADF; @@ -250,26 +259,30 @@ CDev::ioctl(file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - /* fetch a pointer to the driver's private data */ + /* fetch a pointer to the driver's private data */ case DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; return OK; break; + case DEVIOCSPUBBLOCK: _pub_blocked = (arg != 0); return OK; break; + case DEVIOCGPUBBLOCK: return _pub_blocked; break; } /* try the superclass. The different ioctl() function form - * means we need to copy arg */ - unsigned arg2 = arg; + * means we need to copy arg */ + unsigned arg2 = arg; int ret = Device::ioctl(cmd, arg2); - if (ret != -ENODEV) + + if (ret != -ENODEV) { return ret; + } return -ENOTTY; } @@ -305,8 +318,9 @@ CDev::poll(file_t *filp, struct pollfd *fds, bool setup) fds->revents |= fds->events & poll_state(filp); /* yes? post the notification */ - if (fds->revents != 0) - sem_post(fds->sem); + if (fds->revents != 0) { + px4_sem_post(fds->sem); + } } } else { @@ -328,8 +342,9 @@ CDev::poll_notify(pollevent_t events) irqstate_t state = irqsave(); for (unsigned i = 0; i < _max_pollwaiters; i++) - if (nullptr != _pollset[i]) + if (nullptr != _pollset[i]) { poll_notify_one(_pollset[i], events); + } irqrestore(state); } @@ -342,8 +357,9 @@ CDev::poll_notify_one(struct pollfd *fds, pollevent_t events) /* if the state is now interesting, wake the waiter if it's still asleep */ /* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */ - if ((fds->revents != 0) && (fds->sem->semcount <= 0)) - sem_post(fds->sem); + if ((fds->revents != 0) && (fds->sem->semcount <= 0)) { + px4_sem_post(fds->sem); + } } pollevent_t diff --git a/src/drivers/device/device_nuttx.cpp b/src/drivers/device/device_nuttx.cpp index d46901683f6f..12d42d5bfe4a 100644 --- a/src/drivers/device/device_nuttx.cpp +++ b/src/drivers/device/device_nuttx.cpp @@ -38,6 +38,7 @@ */ #include "device.h" +#include "px4_log.h" #include #include @@ -94,7 +95,7 @@ Device::Device(const char *name, _irq_attached(false) { sem_init(&_lock, 0, 1); - + /* setup a default device ID. When bus_type is UNKNOWN the other fields are invalid */ _device_id.devid = 0; @@ -108,8 +109,9 @@ Device::~Device() { sem_destroy(&_lock); - if (_irq_attached) + if (_irq_attached) { unregister_interrupt(_irq); + } } int @@ -125,8 +127,9 @@ Device::init() /* register */ ret = register_interrupt(_irq, this); - if (ret != OK) + if (ret != OK) { goto out; + } _irq_attached = true; } @@ -138,15 +141,17 @@ Device::init() void Device::interrupt_enable() { - if (_irq_attached) + if (_irq_attached) { up_enable_irq(_irq); + } } void Device::interrupt_disable() { - if (_irq_attached) + if (_irq_attached) { up_disable_irq(_irq); + } } void @@ -156,34 +161,6 @@ Device::interrupt(void *context) interrupt_disable(); } -void -Device::log(const char *fmt, ...) -{ - va_list ap; - - printf("[%s] ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); -} - -void -Device::debug(const char *fmt, ...) -{ - va_list ap; - - if (_debug_enabled) { - printf("<%s> ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); - } -} - static int register_interrupt(int irq, Device *owner) { @@ -251,6 +228,7 @@ Device::ioctl(unsigned operation, unsigned &arg) case DEVIOCGDEVICEID: return (int)_device_id.devid; } + return -ENODEV; } diff --git a/src/drivers/device/device_nuttx.h b/src/drivers/device/device_nuttx.h index a3a0640d2d3b..c8f732fab1a3 100644 --- a/src/drivers/device/device_nuttx.h +++ b/src/drivers/device/device_nuttx.h @@ -54,6 +54,9 @@ #include +#define DEVICE_LOG(FMT, ...) PX4_LOG_NAMED(_name, FMT, ##__VA_ARGS__) +#define DEVICE_DEBUG(FMT, ...) PX4_LOG_NAMED_COND(_name, _debug_enabled, FMT, ##__VA_ARGS__) + /** * Namespace encapsulating all device framework classes, functions and data. */ @@ -115,7 +118,7 @@ class __EXPORT Device * @param data The buffer from which values should be read. * @param count The number of items to write. * @return The number of items written on success, negative errno otherwise. - */ + */ virtual int write(unsigned address, void *data, unsigned count); /** @@ -144,13 +147,13 @@ class __EXPORT Device parameter protocol without loss of information. */ struct DeviceStructure { - enum DeviceBusType bus_type:3; - uint8_t bus:5; // which instance of the bus type - uint8_t address; // address on the bus (eg. I2C address) - uint8_t devtype; // device class specific device type - }; + enum DeviceBusType bus_type : 3; + uint8_t bus: 5; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; - union DeviceId { + union DeviceId { struct DeviceStructure devid_s; uint32_t devid; }; @@ -186,33 +189,19 @@ class __EXPORT Device * * Note that we must loop as the wait may be interrupted by a signal. */ - void lock() { + void lock() + { do {} while (sem_wait(&_lock) != 0); } /** * Release the driver lock. */ - void unlock() { + void unlock() + { sem_post(&_lock); } - /** - * Log a message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void log(const char *fmt, ...); - - /** - * Print a debug message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void debug(const char *fmt, ...); - private: int _irq; bool _irq_attached; @@ -429,7 +418,7 @@ class __EXPORT CDev : public Device */ virtual int close_last(file_t *filp); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -438,7 +427,7 @@ class __EXPORT CDev : public Device */ virtual int register_class_devname(const char *class_devname); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -453,7 +442,7 @@ class __EXPORT CDev : public Device * * @return the file system string of the device handle */ - const char* get_devname() { return _devname; } + const char *get_devname() { return _devname; } bool _pub_blocked; /**< true if publishing should be blocked */ @@ -483,8 +472,8 @@ class __EXPORT CDev : public Device int remove_poll_waiter(struct pollfd *fds); /* do not allow copying this class */ - CDev(const CDev&); - CDev operator=(const CDev&); + CDev(const CDev &); + CDev operator=(const CDev &); }; /** @@ -516,7 +505,8 @@ class __EXPORT PIO : public CDev * * @param offset Register offset in bytes from the base address. */ - uint32_t reg(uint32_t offset) { + uint32_t reg(uint32_t offset) + { return *(volatile uint32_t *)(_base + offset); } @@ -526,7 +516,8 @@ class __EXPORT PIO : public CDev * @param offset Register offset in bytes from the base address. * @param value Value to write. */ - void reg(uint32_t offset, uint32_t value) { + void reg(uint32_t offset, uint32_t value) + { *(volatile uint32_t *)(_base + offset) = value; } @@ -540,7 +531,8 @@ class __EXPORT PIO : public CDev * @param clearbits Bits to clear in the register * @param setbits Bits to set in the register */ - void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) + { uint32_t val = reg(offset); val &= ~clearbits; val |= setbits; @@ -555,9 +547,9 @@ class __EXPORT PIO : public CDev // class instance for primary driver of each class enum CLASS_DEVICE { - CLASS_DEVICE_PRIMARY=0, - CLASS_DEVICE_SECONDARY=1, - CLASS_DEVICE_TERTIARY=2 + CLASS_DEVICE_PRIMARY = 0, + CLASS_DEVICE_SECONDARY = 1, + CLASS_DEVICE_TERTIARY = 2 }; #endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/device/device_posix.cpp b/src/drivers/device/device_posix.cpp index 771bee247133..088d7ccdf825 100644 --- a/src/drivers/device/device_posix.cpp +++ b/src/drivers/device/device_posix.cpp @@ -44,6 +44,7 @@ #include #include #include +#include namespace device { @@ -54,8 +55,12 @@ Device::Device(const char *name) : _name(name), _debug_enabled(false) { - sem_init(&_lock, 0, 1); - + int ret = px4_sem_init(&_lock, 0, 1); + + if (ret != 0) { + PX4_WARN("SEM INIT FAIL: ret %d, %s", ret, strerror(errno)); + } + /* setup a default device ID. When bus_type is UNKNOWN the other fields are invalid */ _device_id.devid = 0; @@ -67,7 +72,7 @@ Device::Device(const char *name) : Device::~Device() { - sem_destroy(&_lock); + px4_sem_destroy(&_lock); } int @@ -78,54 +83,27 @@ Device::init() return ret; } -void -Device::log(const char *fmt, ...) -{ - va_list ap; - - PX4_INFO("[%s] ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); -} - -void -Device::debug(const char *fmt, ...) -{ - va_list ap; - - if (_debug_enabled) { - printf("<%s> ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); - } -} - int Device::dev_read(unsigned offset, void *data, unsigned count) { - return -ENODEV; + return -ENODEV; } int Device::dev_write(unsigned offset, void *data, unsigned count) { - return -ENODEV; + return -ENODEV; } int Device::dev_ioctl(unsigned operation, unsigned &arg) { - switch (operation) { - case DEVIOCGDEVICEID: - return (int)_device_id.devid; - } - return -ENODEV; + switch (operation) { + case DEVIOCGDEVICEID: + return (int)_device_id.devid; + } + + return -ENODEV; } } // namespace device diff --git a/src/drivers/device/i2c_nuttx.cpp b/src/drivers/device/i2c_nuttx.cpp index 092ecd144075..5efd2eac234e 100644 --- a/src/drivers/device/i2c_nuttx.cpp +++ b/src/drivers/device/i2c_nuttx.cpp @@ -69,7 +69,7 @@ I2C::I2C(const char *name, _device_id.devid_s.bus = bus; _device_id.devid_s.address = address; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } I2C::~I2C() @@ -90,8 +90,9 @@ I2C::set_bus_clock(unsigned bus, unsigned clock_hz) } if (_bus_clocks[index] > 0) { - // debug("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz); + // DEVICE_DEBUG("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz); } + _bus_clocks[index] = clock_hz; return OK; @@ -107,7 +108,7 @@ I2C::init() _dev = up_i2cinitialize(_bus); if (_dev == nullptr) { - debug("failed to init I2C"); + DEVICE_DEBUG("failed to init I2C"); ret = -ENOENT; goto out; } @@ -121,8 +122,8 @@ I2C::init() if (_bus_clocks[bus_index] > _frequency) { (void)up_i2cuninitialize(_dev); _dev = nullptr; - log("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", - _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); + DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", + _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); ret = -EINVAL; goto out; } @@ -148,7 +149,7 @@ I2C::init() ret = probe(); if (ret != OK) { - debug("probe failed"); + DEVICE_DEBUG("probe failed"); goto out; } @@ -156,19 +157,21 @@ I2C::init() ret = CDev::init(); if (ret != OK) { - debug("cdev init failed"); + DEVICE_DEBUG("cdev init failed"); goto out; } // tell the world where we are - log("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", - _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); + DEVICE_LOG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", + _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); out: + if ((ret != OK) && (_dev != nullptr)) { up_i2cuninitialize(_dev); _dev = nullptr; } + return ret; } @@ -188,7 +191,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re unsigned retry_count = 0; do { - // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + // DEVICE_DEBUG("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); msgs = 0; @@ -208,18 +211,21 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re msgs++; } - if (msgs == 0) + if (msgs == 0) { return -EINVAL; + } ret = I2C_TRANSFER(_dev, &msgv[0], msgs); /* success */ - if (ret == OK) + if (ret == OK) { break; + } /* if we have already retried once, or we are going to give up, then reset the bus */ - if ((retry_count >= 1) || (retry_count >= _retries)) + if ((retry_count >= 1) || (retry_count >= _retries)) { up_i2creset(_dev); + } } while (retry_count++ < _retries); @@ -234,20 +240,23 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs) unsigned retry_count = 0; /* force the device address into the message vector */ - for (unsigned i = 0; i < msgs; i++) + for (unsigned i = 0; i < msgs; i++) { msgv[i].addr = _address; + } do { ret = I2C_TRANSFER(_dev, msgv, msgs); /* success */ - if (ret == OK) + if (ret == OK) { break; + } /* if we have already retried once, or we are going to give up, then reset the bus */ - if ((retry_count >= 1) || (retry_count >= _retries)) + if ((retry_count >= 1) || (retry_count >= _retries)) { up_i2creset(_dev); + } } while (retry_count++ < _retries); diff --git a/src/drivers/device/i2c_nuttx.h b/src/drivers/device/i2c_nuttx.h index 97ab25672c1c..f4aa608857e3 100644 --- a/src/drivers/device/i2c_nuttx.h +++ b/src/drivers/device/i2c_nuttx.h @@ -134,7 +134,8 @@ class __EXPORT I2C : public CDev * * @param address The new bus address to set. */ - void set_address(uint16_t address) { + void set_address(uint16_t address) + { _address = address; _device_id.devid_s.address = _address; } diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index 473307b1cc64..48bb3529e0ed 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -49,7 +49,7 @@ #include #include -#define PX4_SIMULATE_I2C 1 +#define PX4_SIMULATE_I2C 0 static int simulate = PX4_SIMULATE_I2C; namespace device @@ -69,19 +69,21 @@ I2C::I2C(const char *name, _address(address), _fd(-1) { - warnx("I2C::I2C name = %s devname = %s", name, devname); + //warnx("I2C::I2C name = %s devname = %s", name, devname); // fill in _device_id fields for a I2C device _device_id.devid_s.bus_type = DeviceBusType_I2C; _device_id.devid_s.bus = bus; _device_id.devid_s.address = address; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } I2C::~I2C() { if (_fd >= 0) { +#ifndef __PX4_QURT ::close(_fd); +#endif _fd = -1; } } @@ -98,13 +100,14 @@ I2C::init() ret = VDev::init(); if (ret != PX4_OK) { - debug("VDev::init failed"); + DEVICE_DEBUG("VDev::init failed"); return ret; } _fd = px4_open(get_devname(), PX4_F_RDONLY | PX4_F_WRONLY); + if (_fd < 0) { - debug("px4_open failed of device %s", get_devname()); + DEVICE_DEBUG("px4_open failed of device %s", get_devname()); return PX4_ERROR; } @@ -114,15 +117,19 @@ I2C::init() if (simulate) { _fd = 10000; - } - else { + + } else { +#ifndef __PX4_QURT // Open the actual I2C device and map to the virtual dev name _fd = ::open(get_devname(), O_RDWR); + if (_fd < 0) { warnx("could not open %s", get_devname()); px4_errno = errno; return PX4_ERROR; } + +#endif } return ret; @@ -131,6 +138,9 @@ I2C::init() int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { +#ifndef __PX4_LINUX + return 1; +#else struct i2c_msg msgv[2]; unsigned msgs; struct i2c_rdwr_ioctl_data packets; @@ -138,12 +148,12 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re unsigned retry_count = 0; if (_fd < 0) { - warnx("I2C device not opened"); + warnx("I2C device not opened"); return 1; } do { - // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + // DEVICE_DEBUG("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); msgs = 0; if (send_len > 0) { @@ -162,8 +172,9 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re msgs++; } - if (msgs == 0) + if (msgs == 0) { return -EINVAL; + } packets.msgs = msgv; packets.nmsgs = msgs; @@ -171,9 +182,10 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re if (simulate) { //warnx("I2C SIM: transfer_4 on %s", get_devname()); ret = PX4_OK; - } - else { + + } else { ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); + if (ret < 0) { warnx("I2C transfer failed"); return 1; @@ -181,24 +193,30 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re } /* success */ - if (ret == PX4_OK) + if (ret == PX4_OK) { break; + } } while (retry_count++ < _retries); return ret; +#endif } int I2C::transfer(struct i2c_msg *msgv, unsigned msgs) { +#ifndef __PX4_LINUX + return 1; +#else struct i2c_rdwr_ioctl_data packets; int ret; unsigned retry_count = 0; /* force the device address into the message vector */ - for (unsigned i = 0; i < msgs; i++) + for (unsigned i = 0; i < msgs; i++) { msgv[i].addr = _address; + } do { packets.msgs = msgv; @@ -207,32 +225,38 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) if (simulate) { warnx("I2C SIM: transfer_2 on %s", get_devname()); ret = PX4_OK; - } - else { + + } else { ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); } + if (ret < 0) { - warnx("I2C transfer failed"); - return 1; - } + warnx("I2C transfer failed"); + return 1; + } /* success */ - if (ret == PX4_OK) + if (ret == PX4_OK) { break; + } } while (retry_count++ < _retries); return ret; +#endif } int I2C::ioctl(device::file_t *filp, int cmd, unsigned long arg) { //struct i2c_rdwr_ioctl_data *packets = (i2c_rdwr_ioctl_data *)(void *)arg; - switch (cmd) { +#ifdef __PX4_LINUX + case I2C_RDWR: - warnx("Use I2C::transfer, not ioctl"); + warnx("Use I2C::transfer, not ioctl"); return 0; +#endif + default: /* give it to the superclass */ return VDev::ioctl(filp, cmd, arg); @@ -243,21 +267,29 @@ ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen) { if (simulate) { // FIXME no idea what this should be - warnx ("2C SIM I2C::read"); + warnx("2C SIM I2C::read"); return 0; } +#ifndef __PX4_QURT return ::read(_fd, buffer, buflen); +#else + return 0; +#endif } ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen) { if (simulate) { - warnx ("2C SIM I2C::write"); + warnx("2C SIM I2C::write"); return buflen; } +#ifndef __PX4_QURT return ::write(_fd, buffer, buflen); +#else + return buflen; +#endif } } // namespace device diff --git a/src/drivers/device/i2c_posix.h b/src/drivers/device/i2c_posix.h index c05100ae43c6..bee8d6a38a70 100644 --- a/src/drivers/device/i2c_posix.h +++ b/src/drivers/device/i2c_posix.h @@ -127,8 +127,8 @@ class __EXPORT I2C : public VDev int _fd; std::string _dname; - I2C(const device::I2C&); - I2C operator=(const device::I2C&); + I2C(const device::I2C &); + I2C operator=(const device::I2C &); }; } // namespace device diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp new file mode 100644 index 000000000000..d6bdfbf5a67e --- /dev/null +++ b/src/drivers/device/integrator.cpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file integrator.cpp + * + * A resettable integrator + * + * @author Lorenz Meier + */ + +#include "integrator.h" + +Integrator::Integrator(uint64_t auto_reset_interval, bool coning_compensation) : + _auto_reset_interval(auto_reset_interval), + _last_integration(0), + _last_auto(0), + _integral_auto(0.0f, 0.0f, 0.0f), + _integral_read(0.0f, 0.0f, 0.0f), + _last_val(0.0f, 0.0f, 0.0f), + _last_delta(0.0f, 0.0f, 0.0f), + _auto_callback(nullptr), + _coning_comp_on(coning_compensation) +{ + +} + +Integrator::~Integrator() +{ + +} + +bool +Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt) +{ + bool auto_reset = false; + + if (_last_integration == 0) { + /* this is the first item in the integrator */ + _last_integration = timestamp; + _last_auto = timestamp; + _last_val = val; + return false; + } + + // Integrate + double dt = (double)(timestamp - _last_integration) / 1000000.0; + math::Vector<3> i = (val + _last_val) * dt * 0.5f; + + // Apply coning compensation if required + if (_coning_comp_on) { + // Coning compensation derived by Paul Riseborough and Jonathan Challinger, + // following: + // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation + // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf + + i += ((_integral_auto + _last_delta * (1.0f / 6.0f)) % i) * 0.5f; + } + + _integral_auto += i; + _integral_read += i; + + _last_integration = timestamp; + _last_val = val; + _last_delta = i; + + if ((timestamp - _last_auto) > _auto_reset_interval) { + if (_auto_callback) { + /* call the callback */ + _auto_callback(timestamp, _integral_auto); + } + + integral = _integral_auto; + integral_dt = (timestamp - _last_auto); + + auto_reset = true; + _last_auto = timestamp; + _integral_auto(0) = 0.0f; + _integral_auto(1) = 0.0f; + _integral_auto(2) = 0.0f; + } + + return auto_reset; +} + +math::Vector<3> +Integrator::read(bool auto_reset) +{ + math::Vector<3> val = _integral_read; + + if (auto_reset) { + _integral_read(0) = 0.0f; + _integral_read(1) = 0.0f; + _integral_read(2) = 0.0f; + } + + return val; +} diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h new file mode 100644 index 000000000000..b9eb9fd37c81 --- /dev/null +++ b/src/drivers/device/integrator.h @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file integrator.h + * + * A resettable integrator + * + * @author Lorenz Meier + */ + +#pragma once + +#include + +class __EXPORT Integrator +{ +public: + Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false); + virtual ~Integrator(); + + /** + * Put an item into the integral. + * + * @param timestamp Timestamp of the current value + * @param val Item to put + * @param integral Current integral in case the integrator did reset, else the value will not be modified + * @return true if putting the item triggered an integral reset + * and the integral should be published + */ + bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt); + + /** + * Get the current integral value + * + * @return the integral since the last auto-reset + */ + math::Vector<3> get() { return _integral_auto; } + + /** + * Read from the integral + * + * @param auto_reset Reset the integral to zero on read + * @return the integral since the last read-reset + */ + math::Vector<3> read(bool auto_reset); + + /** + * Get current integral start time + */ + uint64_t current_integral_start() { return _last_auto; } + +private: + uint64_t _auto_reset_interval; /**< the interval after which the content will be published and the integrator reset */ + uint64_t _last_integration; /**< timestamp of the last integration step */ + uint64_t _last_auto; /**< last auto-announcement of integral value */ + math::Vector<3> _integral_auto; /**< the integrated value which auto-resets after _auto_reset_interval */ + math::Vector<3> _integral_read; /**< the integrated value since the last read */ + math::Vector<3> _last_val; /**< previously integrated last value */ + math::Vector<3> _last_delta; /**< last local delta */ + void (*_auto_callback)(uint64_t, math::Vector<3>); /**< the function callback for auto-reset */ + bool _coning_comp_on; /**< coning compensation */ + + /* we don't want this class to be copied */ + Integrator(const Integrator &); + Integrator operator=(const Integrator &); +}; diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index c206a5037c70..0585e814182d 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -38,6 +38,7 @@ ifeq ($(PX4_TARGET_OS),nuttx) SRCS = \ device_nuttx.cpp \ + integrator.cpp \ cdev.cpp \ i2c_nuttx.cpp \ pio.cpp \ @@ -51,5 +52,6 @@ SRCS = \ vdev_posix.cpp \ i2c_posix.cpp \ sim.cpp \ - ringbuffer.cpp + ringbuffer.cpp \ + integrator.cpp endif diff --git a/src/drivers/device/ringbuffer.cpp b/src/drivers/device/ringbuffer.cpp index c30e0a54df8b..5843312915ba 100644 --- a/src/drivers/device/ringbuffer.cpp +++ b/src/drivers/device/ringbuffer.cpp @@ -46,15 +46,16 @@ namespace ringbuffer RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : _num_items(num_items), _item_size(item_size), - _buf(new char[(_num_items+1) * item_size]), - _head(_num_items), - _tail(_num_items) + _buf(new char[(_num_items + 1) * item_size]), + _head(_num_items), + _tail(_num_items) {} RingBuffer::~RingBuffer() { - if (_buf != nullptr) + if (_buf != nullptr) { delete[] _buf; + } } unsigned @@ -84,20 +85,25 @@ RingBuffer::size() void RingBuffer::flush() { - while (!empty()) + while (!empty()) { get(NULL); + } } bool RingBuffer::put(const void *val, size_t val_size) { unsigned next = _next(_head); + if (next != _tail) { - if ((val_size == 0) || (val_size > _item_size)) + if ((val_size == 0) || (val_size > _item_size)) { val_size = _item_size; + } + memcpy(&_buf[_head * _item_size], val, val_size); _head = next; return true; + } else { return false; } @@ -169,11 +175,14 @@ RingBuffer::force(const void *val, size_t val_size) bool overwrote = false; for (;;) { - if (put(val, val_size)) + if (put(val, val_size)) { break; + } + get(NULL); overwrote = true; } + return overwrote; } @@ -246,6 +255,7 @@ static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned *a = c; return true; } + return false; } @@ -259,8 +269,9 @@ RingBuffer::get(void *val, size_t val_size) unsigned candidate; unsigned next; - if ((val_size == 0) || (val_size > _item_size)) + if ((val_size == 0) || (val_size > _item_size)) { val_size = _item_size; + } do { /* decide which element we think we're going to read */ @@ -270,13 +281,15 @@ RingBuffer::get(void *val, size_t val_size) next = _next(candidate); /* go ahead and read from this index */ - if (val != NULL) + if (val != NULL) { memcpy(val, &_buf[candidate * _item_size], val_size); + } /* if the tail pointer didn't change, we got our item */ } while (!__PX4_SBCAP(&_tail, candidate, next)); return true; + } else { return false; } @@ -377,10 +390,12 @@ bool RingBuffer::resize(unsigned new_size) { char *old_buffer; - char *new_buffer = new char [(new_size+1) * _item_size]; + char *new_buffer = new char [(new_size + 1) * _item_size]; + if (new_buffer == nullptr) { return false; } + old_buffer = _buf; _buf = new_buffer; _num_items = new_size; diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 4fcdaf47fa1b..32077edb64b1 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -46,7 +46,8 @@ namespace ringbuffer __EXPORT { -class RingBuffer { +class RingBuffer +{ public: RingBuffer(unsigned ring_size, size_t entry_size); virtual ~RingBuffer(); @@ -171,8 +172,8 @@ class RingBuffer { unsigned _next(unsigned index); /* we don't want this class to be copied */ - RingBuffer(const RingBuffer&); - RingBuffer operator=(const RingBuffer&); + RingBuffer(const RingBuffer &); + RingBuffer operator=(const RingBuffer &); }; } // namespace ringbuffer diff --git a/src/drivers/device/sim.cpp b/src/drivers/device/sim.cpp index 787a3826c929..5b089af935ea 100644 --- a/src/drivers/device/sim.cpp +++ b/src/drivers/device/sim.cpp @@ -69,7 +69,7 @@ SIM::SIM(const char *name, _device_id.devid_s.bus = bus; _device_id.devid_s.address = address; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } SIM::~SIM() @@ -104,7 +104,7 @@ SIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re if (recv_len > 0) { PX4_DEBUG("SIM: receiving %d bytes", recv_len); - + // TODO - write data to recv; } diff --git a/src/drivers/device/sim.h b/src/drivers/device/sim.h index 139967f6e854..1e84af386399 100644 --- a/src/drivers/device/sim.h +++ b/src/drivers/device/sim.h @@ -37,7 +37,7 @@ * Base class for devices on simulation bus. */ -#pragma once +#pragma once #include "vdev.h" @@ -98,14 +98,14 @@ class __EXPORT SIM : public Device * otherwise. */ virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); + uint8_t *recv, unsigned recv_len); private: uint16_t _address; - const char * _devname; + const char *_devname; - SIM(const device::SIM&); - SIM operator=(const device::SIM&); + SIM(const device::SIM &); + SIM operator=(const device::SIM &); }; } // namespace device diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 14f6d5706c64..ce81878102f9 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -80,7 +80,7 @@ SPI::SPI(const char *name, _device_id.devid_s.bus = bus; _device_id.devid_s.address = (uint8_t)device; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } SPI::~SPI() @@ -94,11 +94,12 @@ SPI::init() int ret = OK; /* attach to the spi bus */ - if (_dev == nullptr) + if (_dev == nullptr) { _dev = up_spiinitialize(_bus); + } if (_dev == nullptr) { - debug("failed to init SPI"); + DEVICE_DEBUG("failed to init SPI"); ret = -ENOENT; goto out; } @@ -110,7 +111,7 @@ SPI::init() ret = probe(); if (ret != OK) { - debug("probe failed"); + DEVICE_DEBUG("probe failed"); goto out; } @@ -118,12 +119,12 @@ SPI::init() ret = CDev::init(); if (ret != OK) { - debug("cdev init failed"); + DEVICE_DEBUG("cdev init failed"); goto out; } /* tell the workd where we are */ - log("on SPI bus %d at %d (%u KHz)", _bus, _device, _frequency / 1000); + DEVICE_LOG("on SPI bus %d at %d (%u KHz)", _bus, _device, _frequency / 1000); out: return ret; @@ -141,34 +142,37 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) { int result; - if ((send == nullptr) && (recv == nullptr)) + if ((send == nullptr) && (recv == nullptr)) { return -EINVAL; + } LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode; /* lock the bus as required */ switch (mode) { default: - case LOCK_PREEMPTION: - { + case LOCK_PREEMPTION: { irqstate_t state = irqsave(); result = _transfer(send, recv, len); irqrestore(state); } break; + case LOCK_THREADS: SPI_LOCK(_dev, true); result = _transfer(send, recv, len); SPI_LOCK(_dev, false); break; + case LOCK_NONE: result = _transfer(send, recv, len); break; } + return result; } -void +void SPI::set_frequency(uint32_t frequency) { _frequency = frequency; diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 2f44f3cafaee..9c3bf36f2550 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -109,12 +109,12 @@ class __EXPORT SPI : public VDev * Set the SPI bus frequency * This is used to change frequency on the fly. Some sensors * (such as the MPU6000) need a lower frequency for setup - * registers and can handle higher frequency for sensor + * registers and can handle higher frequency for sensor * value registers * * @param frequency Frequency to set (Hz) */ - void set_frequency(uint32_t frequency); + void set_frequency(uint32_t frequency); /** * Locking modes supported by the driver. @@ -134,8 +134,8 @@ class __EXPORT SPI : public VDev struct spi_dev_s *_dev; /* this class does not allow copying */ - SPI(const SPI&); - SPI operator=(const SPI&); + SPI(const SPI &); + SPI operator=(const SPI &); protected: int _bus; diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 8ff8ae1fc80e..4aa58447f358 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -54,8 +54,9 @@ struct px4_dev_t { char *name; void *cdev; - px4_dev_t(const char *n, void *c) : cdev(c) { - name = strdup(n); + px4_dev_t(const char *n, void *c) : cdev(c) + { + name = strdup(n); } ~px4_dev_t() { free(name); } @@ -64,8 +65,9 @@ struct px4_dev_t { px4_dev_t() {} }; -#define PX4_MAX_DEV 50 +#define PX4_MAX_DEV 500 static px4_dev_t *devmap[PX4_MAX_DEV]; +pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER; /* * The standard NuttX operation dispatch table can't call C++ member functions @@ -85,21 +87,26 @@ VDev::VDev(const char *name, _open_count(0) { PX4_DEBUG("VDev::VDev"); - for (unsigned i = 0; i < _max_pollwaiters; i++) + + for (unsigned i = 0; i < _max_pollwaiters; i++) { _pollset[i] = nullptr; + } } VDev::~VDev() { PX4_DEBUG("VDev::~VDev"); - if (_registered) + + if (_registered) { unregister_driver(_devname); + } } int VDev::register_class_devname(const char *class_devname) { PX4_DEBUG("VDev::register_class_devname %s", class_devname); + if (class_devname == nullptr) { return -EINVAL; } @@ -111,13 +118,16 @@ VDev::register_class_devname(const char *class_devname) char name[32]; snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); ret = register_driver(name, (void *)this); - if (ret == OK) break; + + if (ret == OK) { break; } + class_instance++; } if (class_instance == 4) { return ret; } + return class_instance; } @@ -127,17 +137,23 @@ VDev::register_driver(const char *name, void *data) PX4_DEBUG("VDev::register_driver %s", name); int ret = -ENOSPC; - if (name == NULL || data == NULL) + if (name == NULL || data == NULL) { return -EINVAL; + } // Make sure the device does not already exist // FIXME - convert this to a map for efficiency - for (int i=0;iname,name) == 0)) { + + pthread_mutex_lock(&devmutex); + + for (int i = 0; i < PX4_MAX_DEV; ++i) { + if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) { + pthread_mutex_unlock(&devmutex); return -EEXIST; } } - for (int i=0;iname) == 0)) { delete devmap[i]; devmap[i] = NULL; @@ -169,6 +192,9 @@ VDev::unregister_driver(const char *name) break; } } + + pthread_mutex_unlock(&devmutex); + return ret; } @@ -178,14 +204,21 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc PX4_DEBUG("VDev::unregister_class_devname"); char name[32]; snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); - for (int i=0;iname,name) == 0) { + + pthread_mutex_lock(&devmutex); + + for (int i = 0; i < PX4_MAX_DEV; ++i) { + if (devmap[i] && strcmp(devmap[i]->name, name) == 0) { delete devmap[i]; PX4_DEBUG("Unregistered class DEV %s", name); devmap[i] = NULL; + pthread_mutex_unlock(&devmutex); return PX4_OK; } } + + pthread_mutex_unlock(&devmutex); + return -EINVAL; } @@ -197,15 +230,17 @@ VDev::init() // base class init first int ret = Device::init(); - if (ret != PX4_OK) + if (ret != PX4_OK) { goto out; + } // now register the driver if (_devname != nullptr) { ret = register_driver(_devname, (void *)this); - if (ret != PX4_OK) + if (ret != PX4_OK) { goto out; + } _registered = true; } @@ -232,8 +267,9 @@ VDev::open(file_t *filep) /* first-open callback may decline the open */ ret = open_first(filep); - if (ret != PX4_OK) + if (ret != PX4_OK) { _open_count--; + } } unlock(); @@ -261,8 +297,9 @@ VDev::close(file_t *filep) _open_count--; /* callback cannot decline the close */ - if (_open_count == 0) + if (_open_count == 0) { ret = close_last(filep); + } } else { ret = -EBADF; @@ -309,21 +346,26 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg) switch (cmd) { - /* fetch a pointer to the driver's private data */ + /* fetch a pointer to the driver's private data */ case DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; ret = PX4_OK; break; + case DEVIOCSPUBBLOCK: _pub_blocked = (arg != 0); ret = PX4_OK; break; + case DEVIOCGPUBBLOCK: ret = _pub_blocked; break; - case DEVIOCGDEVICEID: - ret = (int)_device_id.devid; + + case DEVIOCGDEVICEID: + ret = (int)_device_id.devid; PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret); + break; + default: break; } @@ -364,8 +406,12 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) fds->revents |= fds->events & poll_state(filep); /* yes? post the notification */ - if (fds->revents != 0) - sem_post(fds->sem); + if (fds->revents != 0) { + px4_sem_post(fds->sem); + } + + } else { + PX4_WARN("Store Poll Waiter error."); } } else { @@ -389,8 +435,9 @@ VDev::poll_notify(pollevent_t events) lock(); for (unsigned i = 0; i < _max_pollwaiters; i++) - if (nullptr != _pollset[i]) + if (nullptr != _pollset[i]) { poll_notify_one(_pollset[i], events); + } unlock(); } @@ -400,17 +447,18 @@ VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) { PX4_DEBUG("VDev::poll_notify_one"); int value; - sem_getvalue(fds->sem, &value); + px4_sem_getvalue(fds->sem, &value); /* update the reported event set */ fds->revents |= fds->events & events; - PX4_DEBUG(" Events fds=%p %0x %0x %0x %d",fds, fds->revents, fds->events, events, value); + PX4_DEBUG(" Events fds=%p %0x %0x %0x %d", fds, fds->revents, fds->events, events, value); /* if the state is now interesting, wake the waiter if it's still asleep */ /* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */ - if ((fds->revents != 0) && (value <= 0)) - sem_post(fds->sem); + if ((fds->revents != 0) && (value <= 0)) { + px4_sem_post(fds->sem); + } } pollevent_t @@ -428,6 +476,7 @@ VDev::store_poll_waiter(px4_pollfd_struct_t *fds) * Look for a free slot. */ PX4_DEBUG("VDev::store_poll_waiter"); + for (unsigned i = 0; i < _max_pollwaiters; i++) { if (nullptr == _pollset[i]) { @@ -445,6 +494,7 @@ int VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) { PX4_DEBUG("VDev::remove_poll_waiter"); + for (unsigned i = 0; i < _max_pollwaiters; i++) { if (fds == _pollset[i]) { @@ -461,67 +511,92 @@ VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) VDev *VDev::getDev(const char *path) { PX4_DEBUG("VDev::getDev"); - int i=0; - for (; iname, path); //} if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) { + pthread_mutex_unlock(&devmutex); return (VDev *)(devmap[i]->cdev); } } + + pthread_mutex_unlock(&devmutex); + return NULL; } void VDev::showDevices() { - int i=0; - PX4_INFO("Devices:\n"); - for (; iname, "/dev/", 5) == 0) { - PX4_INFO(" %s\n", devmap[i]->name); + PX4_INFO(" %s", devmap[i]->name); } } + + pthread_mutex_unlock(&devmutex); } void VDev::showTopics() { - int i=0; - PX4_INFO("Devices:\n"); - for (; iname, "/obj/", 5) == 0) { - PX4_INFO(" %s\n", devmap[i]->name); + PX4_INFO(" %s", devmap[i]->name); } } + + pthread_mutex_unlock(&devmutex); } void VDev::showFiles() { - int i=0; - PX4_INFO("Files:\n"); - for (; iname, "/obj/", 5) != 0 && - strncmp(devmap[i]->name, "/dev/", 5) != 0) { - PX4_INFO(" %s\n", devmap[i]->name); + strncmp(devmap[i]->name, "/dev/", 5) != 0) { + PX4_INFO(" %s", devmap[i]->name); } } + + pthread_mutex_unlock(&devmutex); } const char *VDev::topicList(unsigned int *next) { - for (;*nextname, "/obj/", 5) == 0) + for (; *next < PX4_MAX_DEV; (*next)++) + if (devmap[*next] && strncmp(devmap[(*next)]->name, "/obj/", 5) == 0) { return devmap[(*next)++]->name; + } + return NULL; } const char *VDev::devList(unsigned int *next) { - for (;*nextname, "/dev/", 5) == 0) + for (; *next < PX4_MAX_DEV; (*next)++) + if (devmap[*next] && strncmp(devmap[(*next)]->name, "/dev/", 5) == 0) { return devmap[(*next)++]->name; + } + return NULL; } -} // namespace device - +} // namespace device \ No newline at end of file diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 7d5b8811b296..d1fb8ff8e170 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -37,7 +37,7 @@ * Definitions for the generic base classes in the virtual device framework. */ -#pragma once +#pragma once /* * Includes here should only cover the needs of the framework definitions. @@ -51,6 +51,9 @@ #include #include +#define DEVICE_LOG(FMT, ...) PX4_LOG_NAMED(_name, FMT, ##__VA_ARGS__) +#define DEVICE_DEBUG(FMT, ...) PX4_LOG_NAMED_COND(_name, _debug_enabled, FMT, ##__VA_ARGS__) + /** * Namespace encapsulating all device framework classes, functions and data. */ @@ -115,17 +118,17 @@ class __EXPORT Device * @param data The buffer from which values should be read. * @param count The number of items to write. * @return The number of items written on success, negative errno otherwise. - */ + */ virtual int dev_write(unsigned address, void *data, unsigned count); - /** - * Perform a device-specific operation. - * - * @param operation The operation to perform. - * @param arg An argument to the operation. - * @return Negative errno on error, OK or positive value on success. - */ - virtual int dev_ioctl(unsigned operation, unsigned &arg); + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform. + * @param arg An argument to the operation. + * @return Negative errno on error, OK or positive value on success. + */ + virtual int dev_ioctl(unsigned operation, unsigned &arg); /* device bus types for DEVID @@ -145,19 +148,20 @@ class __EXPORT Device parameter protocol without loss of information. */ struct DeviceStructure { - enum DeviceBusType bus_type:3; - uint8_t bus:5; // which instance of the bus type - uint8_t address; // address on the bus (eg. I2C address) - uint8_t devtype; // device class specific device type - }; + enum DeviceBusType bus_type : 3; + uint8_t bus: 5; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; - union DeviceId { + union DeviceId { struct DeviceStructure devid_s; uint32_t devid; }; protected: const char *_name; /**< driver name */ + char *_lock_name; /**< name of the semaphore */ bool _debug_enabled; /**< if true, debug messages are printed */ union DeviceId _device_id; /**< device identifier information */ @@ -175,37 +179,23 @@ class __EXPORT Device * * Note that we must loop as the wait may be interrupted by a signal. */ - void lock() { - debug("lock"); - do {} while (sem_wait(&_lock) != 0); + void lock() + { + DEVICE_DEBUG("lock"); + do {} while (px4_sem_wait(&_lock) != 0); } /** * Release the driver lock. */ - void unlock() { - debug("unlock"); - sem_post(&_lock); + void unlock() + { + DEVICE_DEBUG("unlock"); + px4_sem_post(&_lock); } - /** - * Log a message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void log(const char *fmt, ...); - - /** - * Print a debug message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void debug(const char *fmt, ...); - private: - sem_t _lock; + px4_sem_t _lock; /** disable copy construction for this and all subclasses */ Device(const Device &); @@ -337,6 +327,13 @@ class __EXPORT VDev : public Device static const char *devList(unsigned int *next); static const char *topicList(unsigned int *next); + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char *get_devname() { return _devname; } + protected: int register_driver(const char *name, void *data); @@ -400,7 +397,7 @@ class __EXPORT VDev : public Device */ virtual int close_last(file_t *filep); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -409,7 +406,7 @@ class __EXPORT VDev : public Device */ virtual int register_class_devname(const char *class_devname); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -419,13 +416,6 @@ class __EXPORT VDev : public Device */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); - /** - * Get the device name. - * - * @return the file system string of the device handle - */ - const char* get_devname() { return _devname; } - bool _pub_blocked; /**< true if publishing should be blocked */ private: @@ -454,7 +444,7 @@ class __EXPORT VDev : public Device int remove_poll_waiter(px4_pollfd_struct_t *fds); /* do not allow copying this class */ - VDev(const VDev&); + VDev(const VDev &); //VDev operator=(const VDev&); }; @@ -476,7 +466,7 @@ class __EXPORT VPIO : public CDev PIO(const char *name, const char *devname, unsigned long base - ); + ); virtual ~PIO(); virtual int init(); @@ -488,7 +478,8 @@ class __EXPORT VPIO : public CDev * * @param offset Register offset in bytes from the base address. */ - uint32_t reg(uint32_t offset) { + uint32_t reg(uint32_t offset) + { return *(volatile uint32_t *)(_base + offset); } @@ -498,7 +489,8 @@ class __EXPORT VPIO : public CDev * @param offset Register offset in bytes from the base address. * @param value Value to write. */ - void reg(uint32_t offset, uint32_t value) { + void reg(uint32_t offset, uint32_t value) + { *(volatile uint32_t *)(_base + offset) = value; } @@ -512,7 +504,8 @@ class __EXPORT VPIO : public CDev * @param clearbits Bits to clear in the register * @param setbits Bits to set in the register */ - void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) + { uint32_t val = reg(offset); val &= ~clearbits; val |= setbits; @@ -528,8 +521,8 @@ class __EXPORT VPIO : public CDev // class instance for primary driver of each class enum CLASS_DEVICE { - CLASS_DEVICE_PRIMARY=0, - CLASS_DEVICE_SECONDARY=1, - CLASS_DEVICE_TERTIARY=2 + CLASS_DEVICE_PRIMARY = 0, + CLASS_DEVICE_SECONDARY = 1, + CLASS_DEVICE_TERTIARY = 2 }; diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index e9617511538f..c5e1c9a4f50a 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -43,6 +43,7 @@ #include "device.h" #include "vfile.h" +#include #include #include #include @@ -51,272 +52,303 @@ using namespace device; +pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER; + extern "C" { -struct timerData { - sem_t &sem; - struct timespec &ts; - - timerData(sem_t &s, struct timespec &t) : sem(s), ts(t) {} - ~timerData() {} -}; + static void timer_cb(void *data) + { + px4_sem_t *p_sem = (px4_sem_t *)data; + px4_sem_post(p_sem); + PX4_DEBUG("timer_handler: Timer expired"); + } + +#define PX4_MAX_FD 200 + static device::file_t *filemap[PX4_MAX_FD] = {}; -static void *timer_handler(void *data) -{ - struct timerData *td = (struct timerData *)data; + int px4_errno; - if (td->ts.tv_sec) { - sleep(td->ts.tv_sec); + inline bool valid_fd(int fd) + { + pthread_mutex_lock(&filemutex); + bool ret = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); + pthread_mutex_unlock(&filemutex); + return ret; } - usleep(td->ts.tv_nsec/1000); - sem_post(&(td->sem)); - PX4_DEBUG("timer_handler: Timer expired"); - return 0; -} + inline VDev *get_vdev(int fd) + { + pthread_mutex_lock(&filemutex); + bool valid = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); + VDev *dev; -#define PX4_MAX_FD 100 -static device::file_t *filemap[PX4_MAX_FD] = {}; + if (valid) { + dev = (VDev *)(filemap[fd]->vdev); -int px4_errno; + } else { + dev = nullptr; + } -inline bool valid_fd(int fd) -{ - return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); -} + pthread_mutex_unlock(&filemutex); + return dev; + } -int px4_open(const char *path, int flags, ...) -{ - PX4_DEBUG("px4_open"); - VDev *dev = VDev::getDev(path); - int ret = 0; - int i; - mode_t mode; - - if (!dev && (flags & (PX4_F_WRONLY|PX4_F_CREAT)) != 0 && - strncmp(path, "/obj/", 5) != 0 && - strncmp(path, "/dev/", 5) != 0) + int px4_open(const char *path, int flags, ...) { - va_list p; - va_start(p, flags); - mode = va_arg(p, mode_t); - va_end(p); - - // Create the file - PX4_DEBUG("Creating virtual file %s", path); - dev = VFile::createFile(path, mode); - } - if (dev) { - for (i=0; iopen(filemap[i]); + + if (dev) { + + pthread_mutex_lock(&filemutex); + + for (i = 0; i < PX4_MAX_FD; ++i) { + if (filemap[i] == 0) { + filemap[i] = new device::file_t(flags, dev, i); + break; + } + } + + pthread_mutex_unlock(&filemutex); + + if (i < PX4_MAX_FD) { + ret = dev->open(filemap[i]); + + } else { + PX4_WARN("exceeded maximum number of file descriptors!"); + ret = -ENOENT; + } + + } else { + ret = -EINVAL; } - else { - ret = -ENOENT; + + if (ret < 0) { + px4_errno = -ret; + return -1; } - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - return -1; - } - PX4_DEBUG("px4_open fd = %d", filemap[i]->fd); - return filemap[i]->fd; -} -int px4_close(int fd) -{ - int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_close fd = %d", fd); - ret = dev->close(filemap[fd]); - filemap[fd] = NULL; + PX4_DEBUG("px4_open fd = %d", filemap[i]->fd); + return filemap[i]->fd; } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - ret = PX4_ERROR; - } - return ret; -} -ssize_t px4_read(int fd, void *buffer, size_t buflen) -{ - int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_read fd = %d", fd); - ret = dev->read(filemap[fd], (char *)buffer, buflen); - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - ret = PX4_ERROR; - } - return ret; -} + int px4_close(int fd) + { + int ret; -ssize_t px4_write(int fd, const void *buffer, size_t buflen) -{ - int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_write fd = %d", fd); - ret = dev->write(filemap[fd], (const char *)buffer, buflen); - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - ret = PX4_ERROR; - } - return ret; -} + VDev *dev = get_vdev(fd); -int px4_ioctl(int fd, int cmd, unsigned long arg) -{ - PX4_DEBUG("px4_ioctl fd = %d", fd); - int ret = 0; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - ret = dev->ioctl(filemap[fd], cmd, arg); - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; + if (dev) { + pthread_mutex_lock(&filemutex); + ret = dev->close(filemap[fd]); + filemap[fd] = nullptr; + pthread_mutex_unlock(&filemutex); + PX4_DEBUG("px4_close fd = %d", fd); + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + ret = PX4_ERROR; + } + + return ret; } - - return (ret == 0) ? PX4_OK : PX4_ERROR; -} -int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) -{ - sem_t sem; - int count = 0; - int ret; - unsigned int i; - struct timespec ts; + ssize_t px4_read(int fd, void *buffer, size_t buflen) + { + int ret; - PX4_DEBUG("Called px4_poll timeout = %d", timeout); - sem_init(&sem, 0, 0); + VDev *dev = get_vdev(fd); + + if (dev) { + PX4_DEBUG("px4_read fd = %d", fd); + ret = dev->read(filemap[fd], (char *)buffer, buflen); + + } else { + ret = -EINVAL; + } - // For each fd - for (i=0; ivdev);; - PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); - ret = dev->poll(filemap[fds[i].fd], &fds[i], true); - - if (ret < 0) - break; + int ret; + + VDev *dev = get_vdev(fd); + + if (dev) { + PX4_DEBUG("px4_write fd = %d", fd); + ret = dev->write(filemap[fd], (const char *)buffer, buflen); + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + ret = PX4_ERROR; } + + return ret; } - if (ret >= 0) + int px4_ioctl(int fd, int cmd, unsigned long arg) { - if (timeout >= 0) - { - pthread_t pt; - void *res; - - ts.tv_sec = timeout/1000; - ts.tv_nsec = (timeout % 1000)*1000000; - - // Create a timer to unblock - struct timerData td(sem, ts); - int rv = pthread_create(&pt, NULL, timer_handler, (void *)&td); - if (rv != 0) { - count = -1; - goto cleanup; - } - sem_wait(&sem); - - // Make sure timer thread is killed before sem goes - // out of scope - (void)pthread_cancel(pt); - (void)pthread_join(pt, &res); - } - else - { - sem_wait(&sem); + PX4_DEBUG("px4_ioctl fd = %d", fd); + int ret = 0; + + VDev *dev = get_vdev(fd); + + if (dev) { + ret = dev->ioctl(filemap[fd], cmd, arg); + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; } - // For each fd - for (i=0; ivdev);; - PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); - ret = dev->poll(filemap[fds[i].fd], &fds[i], false); - - if (ret < 0) + if (dev) { + PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], true); + + if (ret < 0) { break; + } + } + } + + if (ret >= 0) { + if (timeout > 0) { + // Use a work queue task + work_s _hpwork; - if (fds[i].revents) - count += 1; + hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000 * timeout); + px4_sem_wait(&sem); + + // Make sure timer thread is killed before sem goes + // out of scope + hrt_work_cancel(&_hpwork); + + } else if (timeout < 0) { + px4_sem_wait(&sem); + } + + // For each fd + for (i = 0; i < nfds; ++i) { + + VDev *dev = get_vdev(fds[i].fd); + + // If fd is valid + if (dev) { + PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], false); + + if (ret < 0) { + break; + } + + if (fds[i].revents) { + count += 1; + } + } } } + + px4_sem_destroy(&sem); + + return count; } -cleanup: - sem_destroy(&sem); + int px4_fsync(int fd) + { + return 0; + } - return count; -} + int px4_access(const char *pathname, int mode) + { + if (mode != F_OK) { + errno = EINVAL; + return -1; + } -int px4_fsync(int fd) -{ - return 0; -} + VDev *dev = VDev::getDev(pathname); + return (dev != nullptr) ? 0 : -1; + } -void px4_show_devices() -{ - VDev::showDevices(); -} + void px4_show_devices() + { + VDev::showDevices(); + } -void px4_show_topics() -{ - VDev::showTopics(); -} + void px4_show_topics() + { + VDev::showTopics(); + } -void px4_show_files() -{ - VDev::showFiles(); -} + void px4_show_files() + { + VDev::showFiles(); + } -const char * px4_get_device_names(unsigned int *handle) -{ - return VDev::devList(handle); -} + const char *px4_get_device_names(unsigned int *handle) + { + return VDev::devList(handle); + } -const char * px4_get_topic_names(unsigned int *handle) -{ - return VDev::topicList(handle); -} + const char *px4_get_topic_names(unsigned int *handle) + { + return VDev::topicList(handle); + } } diff --git a/src/drivers/device/vfile.cpp b/src/drivers/device/vfile.cpp index 2d2d81c816be..e142f626d16a 100644 --- a/src/drivers/device/vfile.cpp +++ b/src/drivers/device/vfile.cpp @@ -48,7 +48,7 @@ VFile::VFile(const char *fname, mode_t mode) : { } -VFile * VFile::createFile(const char *fname, mode_t mode) +VFile *VFile::createFile(const char *fname, mode_t mode) { VFile *me = new VFile(fname, mode); me->register_driver(fname, me); diff --git a/src/drivers/device/vfile.h b/src/drivers/device/vfile.h index d7c5e15d7f8c..6bea62d1f1d8 100644 --- a/src/drivers/device/vfile.h +++ b/src/drivers/device/vfile.h @@ -44,7 +44,8 @@ #include #include -class VFile : public device::VDev { +class VFile : public device::VDev +{ public: static VFile *createFile(const char *fname, mode_t mode); From 5415a82dae084090296780d0e2a7b77c056d3ca4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:02:45 +1100 Subject: [PATCH 201/293] drivers: update headers from upstream --- src/drivers/drv_accel.h | 30 ++--------- src/drivers/drv_baro.h | 18 +------ src/drivers/drv_device.h | 5 ++ src/drivers/drv_gpio.h | 4 +- src/drivers/drv_gyro.h | 30 ++--------- src/drivers/drv_hrt.h | 4 +- src/drivers/drv_mag.h | 28 +---------- src/drivers/drv_pwm_output.h | 33 ++++-------- src/drivers/drv_px4flow.h | 5 -- src/drivers/drv_range_finder.h | 5 -- src/drivers/drv_rc_input.h | 91 ++-------------------------------- src/drivers/drv_sensor.h | 5 ++ src/drivers/drv_tone_alarm.h | 1 + 13 files changed, 41 insertions(+), 218 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 7deccad6cbfd..ab507770b605 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -51,25 +51,8 @@ #define ACCEL1_DEVICE_PATH "/dev/accel1" #define ACCEL2_DEVICE_PATH "/dev/accel2" -/** - * accel report structure. Reads from the device must be in multiples of this - * structure. - */ -struct accel_report { - uint64_t timestamp; - uint64_t error_count; - float x; /**< acceleration in the NED X board axis in m/s^2 */ - float y; /**< acceleration in the NED Y board axis in m/s^2 */ - float z; /**< acceleration in the NED Z board axis in m/s^2 */ - float temperature; /**< temperature in degrees celsius */ - float range_m_s2; /**< range in m/s^2 (+- this value) */ - float scaling; - - int16_t x_raw; - int16_t y_raw; - int16_t z_raw; - int16_t temperature_raw; -}; +#include +#define accel_report sensor_accel_s /** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ struct accel_scale { @@ -81,11 +64,6 @@ struct accel_scale { float z_scale; }; -/* - * ObjDev tag for raw accelerometer data. - */ -ORB_DECLARE(sensor_accel); - /* * ioctl() definitions * @@ -105,10 +83,10 @@ ORB_DECLARE(sensor_accel); /** return the accel internal sample rate in Hz */ #define ACCELIOCGSAMPLERATE _ACCELIOC(1) -/** set the software low-pass filter cut-off in Hz */ +/** set the accel internal lowpass filter to no lower than (arg) Hz */ #define ACCELIOCSLOWPASS _ACCELIOC(2) -/** get the software low-pass filter cut-off in Hz */ +/** return the accel internal lowpass filter in Hz */ #define ACCELIOCGLOWPASS _ACCELIOC(3) /** set the accel scaling constants to the structure pointed to by (arg) */ diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 791e3c5cc1ee..87bf1000ec02 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -50,22 +50,8 @@ #define BARO_BASE_DEVICE_PATH "/dev/baro" #define BARO0_DEVICE_PATH "/dev/baro0" -/** - * baro report structure. Reads from the device must be in multiples of this - * structure. - */ -struct baro_report { - float pressure; - float altitude; - float temperature; - uint64_t timestamp; - uint64_t error_count; -}; - -/* - * ObjDev tag for raw barometer data. - */ -ORB_DECLARE(sensor_baro); +#include +#define baro_report sensor_baro_s /* * ioctl() definitions diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index 3477dde9fd0f..02dbe7af676b 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -66,6 +66,11 @@ #define DEVIOCGDEVICEID _DEVICEIOC(2) #ifdef __PX4_POSIX + +#ifndef SIOCDEVPRIVATE +#define SIOCDEVPRIVATE 1 +#endif + #define DIOC_GETPRIV SIOCDEVPRIVATE #endif diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index cc378472692f..2664256009cf 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -137,8 +137,8 @@ /* no GPIO driver on the PX4_STM32F4DISCOVERY board */ #endif -#ifdef CONFIG_ARCH_BOARD_POSIXTEST -/* no GPIO driver on the POSIXTEST board */ +#ifdef CONFIG_ARCH_BOARD_SITL +/* no GPIO driver on the SITL configuration */ #endif #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 5c9ab1785134..cf5af101fd90 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -51,25 +51,8 @@ #define GYRO1_DEVICE_PATH "/dev/gyro1" #define GYRO2_DEVICE_PATH "/dev/gyro2" -/** - * gyro report structure. Reads from the device must be in multiples of this - * structure. - */ -struct gyro_report { - uint64_t timestamp; - uint64_t error_count; - float x; /**< angular velocity in the NED X board axis in rad/s */ - float y; /**< angular velocity in the NED Y board axis in rad/s */ - float z; /**< angular velocity in the NED Z board axis in rad/s */ - float temperature; /**< temperature in degrees celcius */ - float range_rad_s; - float scaling; - - int16_t x_raw; - int16_t y_raw; - int16_t z_raw; - int16_t temperature_raw; -}; +#include +#define gyro_report sensor_gyro_s /** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */ struct gyro_scale { @@ -81,11 +64,6 @@ struct gyro_scale { float z_scale; }; -/* - * ObjDev tag for raw gyro data. - */ -ORB_DECLARE(sensor_gyro); - /* * ioctl() definitions */ @@ -101,10 +79,10 @@ ORB_DECLARE(sensor_gyro); /** return the gyro internal sample rate in Hz */ #define GYROIOCGSAMPLERATE _GYROIOC(1) -/** set the software low-pass filter cut-off in Hz */ +/** set the gyro internal lowpass filter to no lower than (arg) Hz */ #define GYROIOCSLOWPASS _GYROIOC(2) -/** get the software low-pass filter cut-off in Hz */ +/** set the gyro internal lowpass filter to no lower than (arg) Hz */ #define GYROIOCGLOWPASS _GYROIOC(3) /** set the gyro scaling constants to (arg) */ diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 3ef3dbce9469..496e272d620f 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -39,12 +39,12 @@ #pragma once -#include #include #include +#define __STDC_FORMAT_MACROS #include -#include +#include #include __BEGIN_DECLS diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index e2f8493e38cd..75d914de8e99 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -49,26 +49,8 @@ #define MAG1_DEVICE_PATH "/dev/mag1" #define MAG2_DEVICE_PATH "/dev/mag2" -/** - * mag report structure. Reads from the device must be in multiples of this - * structure. - * - * Output values are in gauss. - */ -struct mag_report { - uint64_t timestamp; - uint64_t error_count; - float x; - float y; - float z; - float range_ga; - float scaling; - float temperature; - - int16_t x_raw; - int16_t y_raw; - int16_t z_raw; -}; +#include +#define mag_report sensor_mag_s /** mag scaling factors; Vout = (Vin * Vscale) + Voffset */ struct mag_scale { @@ -80,12 +62,6 @@ struct mag_scale { float z_scale; }; -/* - * ObjDev tag for raw magnetometer data. - */ -ORB_DECLARE(sensor_mag); - - /* * ioctl() definitions */ diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 8c7efaf22443..bb0424533314 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -61,10 +61,17 @@ __BEGIN_DECLS #define PWM_OUTPUT_BASE_DEVICE_PATH "/dev/pwm_output" #define PWM_OUTPUT0_DEVICE_PATH "/dev/pwm_output0" +#include +#define pwm_output_values output_pwm_s + +#ifndef PWM_OUTPUT_MAX_CHANNELS +#define PWM_OUTPUT_MAX_CHANNELS output_pwm_s::PWM_OUTPUT_MAX_CHANNELS +#endif + /** * Maximum number of PWM output channels supported by the device. */ -#define PWM_OUTPUT_MAX_CHANNELS 16 +//#define PWM_OUTPUT_MAX_CHANNELS 16 /** * Lowest minimum PWM in us @@ -84,7 +91,7 @@ __BEGIN_DECLS /** * Highest maximum PWM in us */ -#define PWM_HIGHEST_MAX 2100 +#define PWM_HIGHEST_MAX 2150 /** * Default maximum PWM in us @@ -94,7 +101,7 @@ __BEGIN_DECLS /** * Lowest PWM allowed as the maximum PWM */ -#define PWM_LOWEST_MAX 1400 +#define PWM_LOWEST_MAX 950 /** * Do not output a channel with this value @@ -107,19 +114,6 @@ __BEGIN_DECLS */ typedef uint16_t servo_position_t; -/** - * Servo output status structure. - * - * May be published to output_pwm, or written to a PWM output - * device. - */ -struct pwm_output_values { - /** desired pulse widths for each of the supported channels */ - servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; - unsigned channel_count; -}; - - /** * RC config values for a channel * @@ -136,11 +130,6 @@ struct pwm_output_rc_config { bool rc_reverse; }; -/* - * ORB tag for PWM outputs. - */ -ORB_DECLARE(output_pwm); - /* * ioctl() definitions * @@ -156,7 +145,7 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_DISARM _PX4_IOC(_PWM_SERVO_BASE, 1) /** get default servo update rate */ -#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) +#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 2) /** set alternate servo update rate */ #define PWM_SERVO_SET_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 3) diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index 63cfe9816a31..90bad2f25de8 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -46,11 +46,6 @@ #define PX4FLOW0_DEVICE_PATH "/dev/px4flow0" -/* - * ObjDev tag for px4flow data. - */ -ORB_DECLARE(optical_flow); - /* * ioctl() definitions * diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 725835cc0a10..7d27c7f33ad6 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -48,11 +48,6 @@ #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" #define MB12XX_MAX_RANGEFINDERS 12 // Maximum number of Maxbotix sensors on bus -/* - * ObjDev tag for distance sensor data. - */ -ORB_DECLARE(distance_sensor); - /* * ioctl() definitions * diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index b82ced384504..b08f10d465bf 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -57,11 +57,6 @@ */ #define RC_INPUT0_DEVICE_PATH "/dev/input_rc0" -/** - * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. - */ -#define RC_INPUT_MAX_CHANNELS 18 - /** * Maximum RSSI value */ @@ -82,10 +77,9 @@ */ #define RC_INPUT_MAX_DEADZONE_US 500 -/** - * @addtogroup topics - * @{ - */ +#include +#define pwm_output_values output_pwm_s +#define rc_input_values input_rc_s /** * Input signal type, value is a control position from zero to 100 @@ -93,85 +87,6 @@ */ typedef uint16_t rc_input_t; -enum RC_INPUT_SOURCE { - RC_INPUT_SOURCE_UNKNOWN = 0, - RC_INPUT_SOURCE_PX4FMU_PPM, - RC_INPUT_SOURCE_PX4IO_PPM, - RC_INPUT_SOURCE_PX4IO_SPEKTRUM, - RC_INPUT_SOURCE_PX4IO_SBUS, - RC_INPUT_SOURCE_PX4IO_ST24, - RC_INPUT_SOURCE_MAVLINK -}; - -/** - * R/C input status structure. - * - * Published to input_rc, may also be published to other names depending - * on the board involved. - */ -struct rc_input_values { - /** publication time */ - uint64_t timestamp_publication; - - /** last valid reception time */ - uint64_t timestamp_last_signal; - - /** number of channels actually being seen */ - uint32_t channel_count; - - /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception */ - int32_t rssi; - - /** - * explicit failsafe flag: true on TX failure or TX out of range , false otherwise. - * Only the true state is reliable, as there are some (PPM) receivers on the market going - * into failsafe without telling us explicitly. - * */ - bool rc_failsafe; - - /** - * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. - * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. - * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. - * */ - bool rc_lost; - - /** - * Number of lost RC frames. - * Note: intended purpose: observe the radio link quality if RSSI is not available - * This value must not be used to trigger any failsafe-alike funtionality. - * */ - uint16_t rc_lost_frame_count; - - /** - * Number of total RC frames. - * Note: intended purpose: observe the radio link quality if RSSI is not available - * This value must not be used to trigger any failsafe-alike funtionality. - * */ - uint16_t rc_total_frame_count; - - /** - * Length of a single PPM frame. - * Zero for non-PPM systems - */ - uint16_t rc_ppm_frame_length; - - /** Input source */ - enum RC_INPUT_SOURCE input_source; - - /** measured pulse widths for each of the supported channels */ - rc_input_t values[RC_INPUT_MAX_CHANNELS]; -}; - -/** - * @} - */ - -/* - * ObjDev tag for R/C inputs. - */ -ORB_DECLARE(input_rc); - #define _RC_INPUT_BASE 0x2b00 /** Fetch R/C input values into (rc_input_values *)arg */ diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index a402f327b774..0c1ab48267a0 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -121,5 +121,10 @@ */ #define SENSORIOCGROTATION _SENSORIOC(6) +/** + * Test the sensor calibration + */ +#define SENSORIOCCALTEST _SENSORIOC(7) + #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index b2e02cdb8f05..4845de69cc1a 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -152,6 +152,7 @@ enum { TONE_EKF_WARNING_TUNE, TONE_BARO_WARNING_TUNE, TONE_SINGLE_BEEP_TUNE, + TONE_HOME_SET, TONE_NUMBER_OF_TUNES }; From 53ac7899b4ba1067022d062b50d8a0d2e28dfd0e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:02:56 +1100 Subject: [PATCH 202/293] ets_airspeed: update from upstream --- src/drivers/ets_airspeed/CMakeLists.txt | 44 +++++++++++++ src/drivers/ets_airspeed/ets_airspeed.cpp | 79 +++++++++++++++-------- 2 files changed, 95 insertions(+), 28 deletions(-) create mode 100644 src/drivers/ets_airspeed/CMakeLists.txt diff --git a/src/drivers/ets_airspeed/CMakeLists.txt b/src/drivers/ets_airspeed/CMakeLists.txt new file mode 100644 index 000000000000..f98941931148 --- /dev/null +++ b/src/drivers/ets_airspeed/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__ets_airspeed + MAIN ets_airspeed + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + ets_airspeed.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index a5a3fd9b2014..83f83896c60f 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -94,7 +94,7 @@ class ETSAirspeed : public Airspeed { public: - ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH); + ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH); protected: @@ -113,8 +113,8 @@ class ETSAirspeed : public Airspeed */ extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path) +ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } @@ -155,15 +155,16 @@ ETSAirspeed::collect() } uint16_t diff_pres_pa_raw = val[1] << 8 | val[0]; - if (diff_pres_pa_raw == 0) { + + if (diff_pres_pa_raw == 0) { // a zero value means the pressure sensor cannot give us a // value. We need to return, and not report a value or the // caller could end up using this value as part of an // average perf_count(_comms_errors); - log("zero value from sensor"); + DEVICE_LOG("zero value from sensor"); return -1; - } + } // The raw value still should be compensated for the known offset diff_pres_pa_raw -= _diff_pres_offset; @@ -175,7 +176,7 @@ ETSAirspeed::collect() differential_pressure_s report; report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); // XXX we may want to smooth out the readings to remove noise. report.differential_pressure_filtered_pa = diff_pres_pa_raw; @@ -210,6 +211,7 @@ ETSAirspeed::cycle() /* perform collection */ ret = collect(); + if (OK != ret) { perf_count(_comms_errors); /* restart the measurement state machine */ @@ -239,8 +241,9 @@ ETSAirspeed::cycle() /* measurement phase */ ret = measure(); + if (OK != ret) { - debug("measure error"); + DEVICE_DEBUG("measure error"); } _sensor_ok = (ret == OK); @@ -287,26 +290,31 @@ start(int i2c_bus) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new ETSAirspeed(i2c_bus); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->Airspeed::init()) + if (OK != g_dev->Airspeed::init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(AIRSPEED0_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); @@ -351,21 +359,24 @@ test() int fd = open(ETS_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -376,22 +387,25 @@ 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("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); } /* reset the sensor polling to its default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { errx(1, "failed to set default rate"); + } errx(0, "PASS"); } @@ -404,14 +418,17 @@ reset() { int fd = open(ETS_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); } @@ -422,8 +439,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -462,32 +480,37 @@ ets_airspeed_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { ets_airspeed::start(i2c_bus); + } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { ets_airspeed::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { ets_airspeed::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { ets_airspeed::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { ets_airspeed::info(); + } ets_airspeed_usage(); exit(0); From a9f3d401710c606891580d7c213bd01997b2b8f5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:03:09 +1100 Subject: [PATCH 203/293] frsky_telemetry: update from upstream --- src/drivers/frsky_telemetry/CMakeLists.txt | 45 +++++++++++++++++++ src/drivers/frsky_telemetry/frsky_data.c | 6 +-- src/drivers/frsky_telemetry/frsky_telemetry.c | 34 +++++++------- 3 files changed, 67 insertions(+), 18 deletions(-) create mode 100644 src/drivers/frsky_telemetry/CMakeLists.txt diff --git a/src/drivers/frsky_telemetry/CMakeLists.txt b/src/drivers/frsky_telemetry/CMakeLists.txt new file mode 100644 index 000000000000..7d6c17f46334 --- /dev/null +++ b/src/drivers/frsky_telemetry/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__frsky_telemetry + MAIN frsky_telemetry + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + frsky_data.c + frsky_telemetry.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index de22a888d38c..d8650c382ea4 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -178,12 +178,12 @@ void frsky_send_frame1(int uart) roundf(raw.accelerometer_m_s2[2] * 1000.0f)); frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, - raw.baro_alt_meter); + raw.baro_alt_meter[0]); frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, - roundf(frac(raw.baro_alt_meter) * 100.0f)); + roundf(frac(raw.baro_alt_meter[0]) * 100.0f)); frsky_send_data(uart, FRSKY_ID_TEMP1, - roundf(raw.baro_temp_celcius)); + roundf(raw.baro_temp_celcius[0])); frsky_send_data(uart, FRSKY_ID_VFAS, roundf(battery.voltage_v * 10.0f)); diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 75d1124a6170..6100f5f1d091 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -83,6 +83,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("ERR: tcgetattr%s: %d\n", uart_name, termios_state); close(uart); @@ -120,9 +121,9 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or static void usage() { fprintf(stderr, - "usage: frsky_telemetry start [-d ]\n" - " frsky_telemetry stop\n" - " frsky_telemetry status\n"); + "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); exit(1); } @@ -139,6 +140,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) argv += 2; int ch; + while ((ch = getopt(argc, argv, "d:")) != EOF) { switch (ch) { case 'd': @@ -155,8 +157,9 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) struct termios uart_config_original; const int uart = frsky_open_uart(device_name, &uart_config_original); - if (uart < 0) + if (uart < 0) { err(1, "could not open %s", device_name); + } /* Subscribe to topics */ frsky_init(); @@ -165,6 +168,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) /* Main thread loop */ unsigned int iteration = 0; + while (!thread_should_exit) { /* Sleep 200 ms */ @@ -174,14 +178,12 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) frsky_send_frame1(uart); /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ - if (iteration % 5 == 0) - { + if (iteration % 5 == 0) { frsky_send_frame2(uart); } /* Send frame 3 (every 5000ms): date, time */ - if (iteration % 25 == 0) - { + if (iteration % 25 == 0) { frsky_send_frame3(uart); iteration = 0; @@ -212,16 +214,17 @@ int frsky_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { /* this is not an error */ - if (thread_running) + if (thread_running) { errx(0, "frsky_telemetry already running"); + } thread_should_exit = false; frsky_task = px4_task_spawn_cmd("frsky_telemetry", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2000, - frsky_telemetry_thread_main, - (char * const *)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + frsky_telemetry_thread_main, + (char *const *)argv); while (!thread_running) { usleep(200); @@ -233,8 +236,9 @@ int frsky_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { /* this is not an error */ - if (!thread_running) + if (!thread_running) { errx(0, "frsky_telemetry already stopped"); + } thread_should_exit = true; From 22a17eebd6504fb944a0710c9b3398ce7590a6de Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:03:20 +1100 Subject: [PATCH 204/293] gimbal: update from upstream --- src/drivers/gimbal/CMakeLists.txt | 43 +++++++++++++++++++++++++++++++ src/drivers/gimbal/gimbal.cpp | 13 +++++----- 2 files changed, 50 insertions(+), 6 deletions(-) create mode 100644 src/drivers/gimbal/CMakeLists.txt diff --git a/src/drivers/gimbal/CMakeLists.txt b/src/drivers/gimbal/CMakeLists.txt new file mode 100644 index 000000000000..ac26a1876ef3 --- /dev/null +++ b/src/drivers/gimbal/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__gimbal + MAIN gimbal + COMPILE_FLAGS + -Os + SRCS + gimbal.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 8085c43a1f0d..40b86d142164 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -331,7 +331,7 @@ Gimbal::cycle() orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL - || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { + || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { _control_cmd = cmd; _control_cmd_set = true; @@ -358,20 +358,21 @@ Gimbal::cycle() if (_control_cmd_set) { unsigned mountMode = _control_cmd.param7; - debug("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); + DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, + (double)_control_cmd.param1, (double)_control_cmd.param2); if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL && - mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1; pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2; yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3; - + updated = true; } if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && - mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4}; math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); @@ -388,7 +389,7 @@ Gimbal::cycle() struct actuator_controls_s controls; - // debug("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw); + // DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw); /* fill in the final control values */ controls.timestamp = hrt_absolute_time(); From d1bca666d7341045def145d5b1cf748047d1a59d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:03:31 +1100 Subject: [PATCH 205/293] gps: update from upstream --- src/drivers/gps/CMakeLists.txt | 48 ++++++ src/drivers/gps/ashtech.cpp | 42 ++++-- src/drivers/gps/ashtech.h | 14 +- src/drivers/gps/gps.cpp | 126 ++++++++++------ src/drivers/gps/mtk.cpp | 8 +- src/drivers/gps/ubx.cpp | 262 +++++++++++++++++++++------------ 6 files changed, 338 insertions(+), 162 deletions(-) create mode 100644 src/drivers/gps/CMakeLists.txt diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt new file mode 100644 index 000000000000..2441ba9ff7d0 --- /dev/null +++ b/src/drivers/gps/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__gps + MAIN gps + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + gps.cpp + gps_helper.cpp + mtk.cpp + ashtech.cpp + ubx.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index c88134492391..322d2755499f 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -37,7 +37,7 @@ ASHTECH::~ASHTECH() int ASHTECH::handle_message(int len) { - char * endp; + char *endp; if (len < 7) { return 0; } @@ -69,7 +69,8 @@ int ASHTECH::handle_message(int len) Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. */ double ashtech_time = 0.0; - int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0; + int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, + local_time_off_min __attribute__((unused)) = 0; if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } @@ -110,12 +111,14 @@ int ASHTECH::handle_message(int len) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = usecs * 1000; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += usecs; + } else { _gps_position->time_utc_usec = 0; } @@ -266,7 +269,8 @@ int ASHTECH::handle_message(int len) double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0; - double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; + double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, + tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; char ns = '?', ew = '?'; if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } @@ -281,7 +285,9 @@ int ASHTECH::handle_message(int len) * or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt. */ lat = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -289,7 +295,9 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -297,7 +305,9 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -349,7 +359,8 @@ int ASHTECH::handle_message(int len) _gps_position->vel_n_m_s = velocity_north; /** GPS ground speed in m/s */ _gps_position->vel_e_m_s = velocity_east; /** GPS ground speed in m/s */ _gps_position->vel_d_m_s = static_cast(-vertic_vel); /** GPS ground speed in m/s */ - _gps_position->cog_rad = track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->cog_rad = + track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ _gps_position->vel_ned_valid = true; /** Flag to indicate if NED speed is valid */ _gps_position->c_variance_rad = 0.1f; _gps_position->timestamp_velocity = hrt_absolute_time(); @@ -381,7 +392,8 @@ int ASHTECH::handle_message(int len) 9 The checksum data, always begins with * */ double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0; + double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, + deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0; if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } @@ -400,7 +412,7 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; } _gps_position->eph = sqrtf(static_cast(lat_err) * static_cast(lat_err) - + static_cast(lon_err) * static_cast(lon_err)); + + static_cast(lon_err) * static_cast(lon_err)); _gps_position->epv = static_cast(alt_err); _gps_position->s_variance_m_s = 0; @@ -472,7 +484,7 @@ int ASHTECH::handle_message(int len) if (this_msg_num == all_msg_num) { end = tot_sv_visible - (this_msg_num - 1) * 4; _gps_position->satellites_used = tot_sv_visible; - _satellite_info->count = SAT_INFO_MAX_SATELLITES; + _satellite_info->count = satellite_info_s::SAT_INFO_MAX_SATELLITES; _satellite_info->timestamp = hrt_absolute_time(); } @@ -571,7 +583,7 @@ int ASHTECH::parse_char(uint8_t b) int iRet = 0; switch (_decode_state) { - /* First, look for sync1 */ + /* First, look for sync1 */ case NME_DECODE_UNINIT: if (b == '$') { _decode_state = NME_DECODE_GOT_SYNC1; @@ -636,13 +648,13 @@ void ASHTECH::decode_init(void) */ const char comm[] = "$PASHS,POP,20\r\n"\ - "$PASHS,NME,ZDA,B,ON,3\r\n"\ - "$PASHS,NME,GGA,B,OFF\r\n"\ - "$PASHS,NME,GST,B,ON,3\r\n"\ - "$PASHS,NME,POS,B,ON,0.05\r\n"\ - "$PASHS,NME,GSV,B,ON,3\r\n"\ - "$PASHS,SPD,A,8\r\n"\ - "$PASHS,SPD,B,9\r\n"; + "$PASHS,NME,ZDA,B,ON,3\r\n"\ + "$PASHS,NME,GGA,B,OFF\r\n"\ + "$PASHS,NME,GST,B,ON,3\r\n"\ + "$PASHS,NME,POS,B,ON,0.05\r\n"\ + "$PASHS,NME,GSV,B,ON,3\r\n"\ + "$PASHS,SPD,A,8\r\n"\ + "$PASHS,SPD,B,9\r\n"; int ASHTECH::configure(unsigned &baudrate) { diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h index 6ba522b9c594..e7b9e40328cc 100644 --- a/src/drivers/gps/ashtech.h +++ b/src/drivers/gps/ashtech.h @@ -42,10 +42,10 @@ #ifndef RECV_BUFFER_SIZE #define RECV_BUFFER_SIZE 512 - -#define SAT_INFO_MAX_SATELLITES 20 #endif +#include + class ASHTECH : public GPS_Helper { @@ -70,11 +70,11 @@ class ASHTECH : public GPS_Helper bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */ /* int _satellites_count; **< Number of satellites info parsed. */ uint8_t count; /**< Number of satellites in satellite info */ - uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ - uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ - uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ + uint8_t svid[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ public: ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 756287dd8164..05ff3e5b91fd 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include #include #include @@ -212,8 +211,9 @@ GPS::~GPS() } /* well, kill it anyway, though this will probably crash */ - if (_task != -1) + if (_task != -1) { task_delete(_task); + } g_dev = nullptr; @@ -225,12 +225,13 @@ GPS::init() int ret = ERROR; /* do regular cdev init */ - if (CDev::init() != OK) + if (CDev::init() != OK) { goto out; + } /* start the GPS driver worker task */ _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr); + SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); @@ -279,7 +280,7 @@ GPS::task_main() _serial_fd = ::open(_port, O_RDWR); if (_serial_fd < 0) { - log("failed to open serial port: %s err: %d", _port, errno); + DEVICE_LOG("failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; _exit(1); @@ -306,11 +307,12 @@ GPS::task_main() _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; - _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * + _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.vel_ned_valid = true; + _report_gps_pos.vel_ned_valid = true; - //no time and satellite information simulated + /* no time and satellite information simulated */ if (!(_pub_blocked)) { @@ -351,38 +353,58 @@ GPS::task_main() unlock(); + /* the Ashtech driver lies about successful configuration and the + * MTK driver is not well tested, so we really only trust the UBX + * driver for an advance publication + */ if (_Helper->configure(_baudrate) == 0) { unlock(); - //Publish initial report that we have access to a GPS - //Make sure to clear any stale data in case driver is reset + /* reset report */ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - _report_gps_pos.timestamp_position = hrt_absolute_time(); - _report_gps_pos.timestamp_variance = hrt_absolute_time(); - _report_gps_pos.timestamp_velocity = hrt_absolute_time(); - _report_gps_pos.timestamp_time = hrt_absolute_time(); - if (!(_pub_blocked)) { - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_mode == GPS_DRIVER_MODE_UBX) { + /* Publish initial report that we have access to a GPS, + * but set all critical state fields to indicate we have + * no valid position lock + */ + + _report_gps_pos.timestamp_time = hrt_absolute_time(); + + /* reset the timestamps for data, because we have no data yet */ + _report_gps_pos.timestamp_position = 0; + _report_gps_pos.timestamp_variance = 0; + _report_gps_pos.timestamp_velocity = 0; + + /* set a massive variance */ + _report_gps_pos.eph = 10000.0f; + _report_gps_pos.epv = 10000.0f; + _report_gps_pos.fix_type = 0; + + if (!(_pub_blocked)) { + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } } - } - // GPS is obviously detected successfully, reset statistics - _Helper->reset_update_rates(); + /* GPS is obviously detected successfully, reset statistics */ + _Helper->reset_update_rates(); + } int helper_ret; + while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { - // lock(); + // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { if (helper_ret & 1) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } + if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub != nullptr) { orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); @@ -491,7 +513,7 @@ void GPS::print_info() { //GPS Mode - if(_fake_gps) { + if (_fake_gps) { warnx("protocol: SIMULATED"); } @@ -505,27 +527,27 @@ GPS::print_info() warnx("protocol: MTK"); break; - case GPS_DRIVER_MODE_ASHTECH: - warnx("protocol: ASHTECH"); - break; + case GPS_DRIVER_MODE_ASHTECH: + warnx("protocol: ASHTECH"); + break; default: break; - } + } } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - warnx("sat info: %s, noise: %d, jamming detected: %s", - (_p_report_sat_info != nullptr) ? "enabled" : "disabled", - _report_gps_pos.noise_per_ms, - _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + warnx("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, - _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, - (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); @@ -558,17 +580,20 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new GPS(uart_path, fake_gps, enable_sat_info); - 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(GPS0_DEVICE_PATH, O_RDONLY); @@ -622,11 +647,13 @@ reset() { int fd = open(GPS0_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, "reset failed"); + } exit(0); } @@ -637,8 +664,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "not running"); + } g_dev->print_info(); @@ -673,39 +701,45 @@ gps_main(int argc, char *argv[]) /* Detect fake gps option */ for (int i = 2; i < argc; i++) { - if (!strcmp(argv[i], "-f")) + if (!strcmp(argv[i], "-f")) { fake_gps = true; + } } /* Detect sat info option */ for (int i = 2; i < argc; i++) { - if (!strcmp(argv[i], "-s")) + if (!strcmp(argv[i], "-s")) { enable_sat_info = true; + } } gps::start(device_name, fake_gps, enable_sat_info); } - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { gps::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { gps::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { gps::reset(); + } /* * Print driver status. */ - if (!strcmp(argv[1], "status")) + if (!strcmp(argv[1], "status")) { gps::info(); + } out: errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 371f17c1a9b8..25991b090a32 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -66,8 +66,9 @@ int MTK::configure(unsigned &baudrate) { /* set baudrate first */ - if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) + if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) { return -1; + } baudrate = MTK_BAUDRATE; @@ -207,8 +208,9 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) } else if (_decode_state == MTK_DECODE_GOT_CK_B) { // Add to checksum - if (_rx_count < 33) + if (_rx_count < 33) { add_byte_to_checksum(b); + } // Fill packet buffer ((uint8_t *)(&packet))[_rx_count] = b; @@ -288,12 +290,14 @@ MTK::handle_message(gps_mtk_packet_t &packet) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL; + } else { _gps_position->time_utc_usec = 0; } diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 064ce20e5f7e..643b339be34e 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -83,7 +83,7 @@ /**** Warning macros, disable to save memory */ #define UBX_WARN(s, ...) {warnx(s, ## __VA_ARGS__);} - +#define UBX_DEBUG(s, ...) {/*warnx(s, ## __VA_ARGS__);*/} UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) : _fd(fd), @@ -199,6 +199,7 @@ UBX::configure(unsigned &baudrate) if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } + #endif /* configure message rates */ @@ -207,41 +208,50 @@ UBX::configure(unsigned &baudrate) /* try to set rate for NAV-PVT */ /* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */ configure_message_rate(UBX_MSG_NAV_PVT, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { _use_nav_pvt = false; + } else { _use_nav_pvt = true; } - UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); + + UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); if (!_use_nav_pvt) { configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_VELNED, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } } configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_MON_HW, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } @@ -269,11 +279,13 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) if (_ack_state == UBX_ACK_GOT_ACK) { ret = 0; // ACK received ok + } else if (report) { if (_ack_state == UBX_ACK_GOT_NAK) { - UBX_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg)); + UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg)); + } else { - UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg)); + UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg)); } } @@ -359,6 +371,7 @@ UBX::parse_char(const uint8_t b) UBX_TRACE_PARSER("\nA"); _decode_state = UBX_DECODE_SYNC2; } + break; /* Expecting Sync2 */ @@ -370,6 +383,7 @@ UBX::parse_char(const uint8_t b) } else { // Sync1 not followed by Sync2: reset parser decode_init(); } + break; /* Expecting Class */ @@ -401,38 +415,48 @@ UBX::parse_char(const uint8_t b) UBX_TRACE_PARSER("F"); add_byte_to_checksum(b); _rx_payload_length |= b << 8; // calculate payload size + if (payload_rx_init() != 0) { // start payload reception // payload will not be handled, discard message decode_init(); + } else { _decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1; } + break; /* Expecting payload */ case UBX_DECODE_PAYLOAD: UBX_TRACE_PARSER("."); add_byte_to_checksum(b); + switch (_rx_msg) { case UBX_MSG_NAV_SVINFO: ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte break; + case UBX_MSG_MON_VER: ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte break; + default: ret = payload_rx_add(b); // add a payload byte break; } + if (ret < 0) { // payload not handled, discard message decode_init(); + } else if (ret > 0) { // payload complete, expecting checksum _decode_state = UBX_DECODE_CHKSUM1; + } else { // expecting more payload, stay in state UBX_DECODE_PAYLOAD } + ret = 0; break; @@ -441,18 +465,22 @@ UBX::parse_char(const uint8_t b) if (_rx_ck_a != b) { UBX_WARN("ubx checksum err"); decode_init(); + } else { _decode_state = UBX_DECODE_CHKSUM2; } + break; /* Expecting second checksum byte */ case UBX_DECODE_CHKSUM2: if (_rx_ck_b != b) { UBX_WARN("ubx checksum err"); + } else { ret = payload_rx_done(); // finish payload processing } + decode_init(); break; @@ -475,83 +503,116 @@ UBX::payload_rx_init() switch (_rx_msg) { case UBX_MSG_NAV_PVT: - if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ - && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */ + if ((_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ + && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) { /* u-blox 8+ msg format */ _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (!_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (!_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + } + break; case UBX_MSG_NAV_POSLLH: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_SOL: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_TIMEUTC: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_SVINFO: - if (_satellite_info == nullptr) - _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else - memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info + if (_satellite_info == nullptr) { + _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else { + memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info + } + break; case UBX_MSG_NAV_VELNED: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_MON_VER: break; // unconditionally handle this message case UBX_MSG_MON_HW: - if ( (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ - && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) /* u-blox 7+ msg format */ + if ((_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ + && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) { /* u-blox 7+ msg format */ _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + } + break; case UBX_MSG_ACK_ACK: - if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + + } else if (_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + } + break; case UBX_MSG_ACK_NAK: - if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + + } else if (_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + } + break; default: @@ -566,7 +627,7 @@ UBX::payload_rx_init() break; case UBX_RXMSG_DISABLE: // disable unexpected messages - UBX_WARN("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length); + UBX_DEBUG("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length); { hrt_abstime t = hrt_absolute_time(); @@ -574,7 +635,7 @@ UBX::payload_rx_init() if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { /* don't attempt for every message to disable, some might not be disabled */ _disable_cmd_last = t; - UBX_WARN("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg)); + UBX_DEBUG("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg)); configure_message_rate(_rx_msg, 0); } } @@ -624,32 +685,39 @@ UBX::payload_rx_add_nav_svinfo(const uint8_t b) if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { // Fill Part 1 buffer _buf.raw[_rx_payload_index] = b; + } else { if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { // Part 1 complete: decode Part 1 buffer - _satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, SAT_INFO_MAX_SATELLITES); - UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); + _satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, satellite_info_s::SAT_INFO_MAX_SATELLITES); + UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, + (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); } - if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) { + + if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof( + ubx_payload_rx_nav_svinfo_part2_t)) { // Still room in _satellite_info: fill Part 2 buffer - unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t); + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof( + ubx_payload_rx_nav_svinfo_part2_t); _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) { // Part 2 complete: decode Part 2 buffer - unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t); + unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof( + ubx_payload_rx_nav_svinfo_part2_t); _satellite_info->used[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01); _satellite_info->snr[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno); _satellite_info->elevation[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev); _satellite_info->azimuth[sat_index] = (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f); _satellite_info->svid[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid); UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u\n", - (unsigned)sat_index + 1, - (unsigned)_satellite_info->used[sat_index], - (unsigned)_satellite_info->snr[sat_index], - (unsigned)_satellite_info->elevation[sat_index], - (unsigned)_satellite_info->azimuth[sat_index], - (unsigned)_satellite_info->svid[sat_index] - ); + (unsigned)sat_index + 1, + (unsigned)_satellite_info->used[sat_index], + (unsigned)_satellite_info->snr[sat_index], + (unsigned)_satellite_info->elevation[sat_index], + (unsigned)_satellite_info->azimuth[sat_index], + (unsigned)_satellite_info->svid[sat_index] + ); } } } @@ -672,21 +740,25 @@ UBX::payload_rx_add_mon_ver(const uint8_t b) if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) { // Fill Part 1 buffer _buf.raw[_rx_payload_index] = b; + } else { if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) { // Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT); _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version); - UBX_WARN("VER hash 0x%08x", _ubx_version); - UBX_WARN("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); - UBX_WARN("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion); + UBX_DEBUG("VER hash 0x%08x", _ubx_version); + UBX_DEBUG("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); + UBX_DEBUG("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion); } + // fill Part 2 buffer - unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t); + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof( + ubx_payload_rx_mon_ver_part2_t); _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) { // Part 2 complete: decode Part 2 buffer - UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); + UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); } } @@ -717,13 +789,11 @@ UBX::payload_rx_done(void) UBX_TRACE_RXMSG("Rx NAV-PVT\n"); //Check if position fix flag is good - if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) - { + if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) { _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; _gps_position->vel_ned_valid = true; - } - else - { + + } else { _gps_position->fix_type = 0; _gps_position->vel_ned_valid = false; } @@ -748,10 +818,9 @@ UBX::payload_rx_done(void) _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; //Check if time and date fix flags are good - if( (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) - && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) - && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) - { + if ((_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) { /* convert to unix timestamp */ struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; @@ -770,12 +839,14 @@ UBX::payload_rx_done(void) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { _gps_position->time_utc_usec = 0; } @@ -827,8 +898,7 @@ UBX::payload_rx_done(void) case UBX_MSG_NAV_TIMEUTC: UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); - if(_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) - { + if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) { // convert to unix timestamp struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900; @@ -849,12 +919,14 @@ UBX::payload_rx_done(void) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { _gps_position->time_utc_usec = 0; } @@ -922,6 +994,7 @@ UBX::payload_rx_done(void) ret = 0; // don't handle message break; } + break; case UBX_MSG_ACK_ACK: @@ -999,39 +1072,44 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len header.length = length; // Calculate checksum - calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes - if (payload != nullptr) + calc_checksum(((uint8_t *)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes + + if (payload != nullptr) { calc_checksum(payload, length, &checksum); + } // Send message write(_fd, (const void *)&header, sizeof(header)); - if (payload != nullptr) + + if (payload != nullptr) { write(_fd, (const void *)payload, length); + } + write(_fd, (const void *)&checksum, sizeof(checksum)); } uint32_t UBX::fnv1_32_str(uint8_t *str, uint32_t hval) { - uint8_t *s = str; + uint8_t *s = str; - /* - * FNV-1 hash each octet in the buffer - */ - while (*s) { + /* + * FNV-1 hash each octet in the buffer + */ + while (*s) { - /* multiply by the 32 bit FNV magic prime mod 2^32 */ + /* multiply by the 32 bit FNV magic prime mod 2^32 */ #if defined(NO_FNV_GCC_OPTIMIZATION) - hval *= FNV1_32_PRIME; + hval *= FNV1_32_PRIME; #else - hval += (hval<<1) + (hval<<4) + (hval<<7) + (hval<<8) + (hval<<24); + hval += (hval << 1) + (hval << 4) + (hval << 7) + (hval << 8) + (hval << 24); #endif - /* xor the bottom with the current octet */ - hval ^= (uint32_t)*s++; - } + /* xor the bottom with the current octet */ + hval ^= (uint32_t) * s++; + } - /* return our new hash value */ - return hval; + /* return our new hash value */ + return hval; } From 1b0289d8a9573661a4a16601112b9bd13641905d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:03:43 +1100 Subject: [PATCH 206/293] hmc5883: update from upstream --- src/drivers/hmc5883/CMakeLists.txt | 47 ++++ src/drivers/hmc5883/hmc5883.cpp | 325 +++++++++++++++++----------- src/drivers/hmc5883/hmc5883.h | 4 +- src/drivers/hmc5883/hmc5883_i2c.cpp | 22 +- src/drivers/hmc5883/hmc5883_spi.cpp | 28 +-- 5 files changed, 279 insertions(+), 147 deletions(-) create mode 100644 src/drivers/hmc5883/CMakeLists.txt diff --git a/src/drivers/hmc5883/CMakeLists.txt b/src/drivers/hmc5883/CMakeLists.txt new file mode 100644 index 000000000000..6f2f98a822f5 --- /dev/null +++ b/src/drivers/hmc5883/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__hmc5883 + MAIN hmc5883 + STACK 1200 + COMPILE_FLAGS + -Weffc++ + -Os + SRCS + hmc5883_i2c.cpp + hmc5883_spi.cpp + hmc5883.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index cd6badd84c83..cdea2a4e0233 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -321,25 +321,25 @@ class HMC5883 : public device::CDev * * @return 0 if calibration is ok, 1 else */ - int check_calibration(); + int check_calibration(); - /** - * Check the current scale calibration - * - * @return 0 if scale calibration is ok, 1 else - */ - int check_scale(); + /** + * Check the current scale calibration + * + * @return 0 if scale calibration is ok, 1 else + */ + int check_scale(); - /** - * Check the current offset calibration - * - * @return 0 if offset calibration is ok, 1 else - */ - int check_offset(); + /** + * Check the current offset calibration + * + * @return 0 if offset calibration is ok, 1 else + */ + int check_offset(); /* this class has pointer data members, do not allow copying it */ - HMC5883(const HMC5883&); - HMC5883 operator=(const HMC5883&); + HMC5883(const HMC5883 &); + HMC5883 operator=(const HMC5883 &); }; /* @@ -397,11 +397,13 @@ HMC5883::~HMC5883() /* make sure we are truly inactive */ stop(); - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance); + } // free perf counters perf_free(_sample_perf); @@ -417,15 +419,18 @@ HMC5883::init() int ret = ERROR; ret = CDev::init(); + if (ret != OK) { - debug("CDev init failed"); + DEVICE_DEBUG("CDev init failed"); goto out; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - if (_reports == nullptr) + + if (_reports == nullptr) { goto out; + } /* reset the device configuration */ reset(); @@ -489,14 +494,16 @@ int HMC5883::set_range(unsigned range) */ ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } uint8_t range_bits_in = 0; ret = read_reg(ADDR_CONF_B, range_bits_in); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } return !(range_bits_in == (_range_bits << 5)); } @@ -512,15 +519,19 @@ void HMC5883::check_range(void) uint8_t range_bits_in = 0; ret = read_reg(ADDR_CONF_B, range_bits_in); + if (OK != ret) { perf_count(_comms_errors); return; } - if (range_bits_in != (_range_bits<<5)) { + + if (range_bits_in != (_range_bits << 5)) { perf_count(_range_errors); ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } } } @@ -535,15 +546,19 @@ void HMC5883::check_conf(void) uint8_t conf_reg_in = 0; ret = read_reg(ADDR_CONF_A, conf_reg_in); + if (OK != ret) { perf_count(_comms_errors); return; } + if (conf_reg_in != _conf_reg) { perf_count(_conf_errors); ret = write_reg(ADDR_CONF_A, _conf_reg); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } } } @@ -555,8 +570,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) 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) { @@ -611,77 +627,84 @@ HMC5883::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; + 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; + 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); + 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(HMC5883_CONVERSION_INTERVAL); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(HMC5883_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; - } + return OK; + } /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + 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); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) - return -EINVAL; + /* check against maximum rate */ + if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) { + return -EINVAL; + } - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval for next measurement */ + _measure_ticks = ticks; - /* 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; + return OK; + } } } - } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } - return 1000000/TICK2USEC(_measure_ticks); + return 1000000 / TICK2USEC(_measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) + if ((arg < 1) || (arg > 100)) { return -EINVAL; + } irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } + irqrestore(flags); return OK; @@ -699,7 +722,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCGSAMPLERATE: /* same as pollrate because device is in single measurement mode*/ - return 1000000/TICK2USEC(_measure_ticks); + return 1000000 / TICK2USEC(_measure_ticks); case MAGIOCSRANGE: return set_range(arg); @@ -734,7 +757,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return check_calibration(); case MAGIOCGEXTERNAL: - debug("MAGIOCGEXTERNAL in main driver"); + DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver"); return _interface->ioctl(cmd, dummy); case MAGIOCSTEMPCOMP: @@ -789,7 +812,7 @@ HMC5883::cycle() /* perform collection */ if (OK != collect()) { - debug("collection error"); + DEVICE_DEBUG("collection error"); /* restart the measurement state machine */ start(); return; @@ -815,8 +838,9 @@ HMC5883::cycle() } /* measurement phase */ - if (OK != measure()) - debug("measure error"); + if (OK != measure()) { + DEVICE_DEBUG("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -839,8 +863,9 @@ HMC5883::measure() */ ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } return ret; } @@ -872,7 +897,7 @@ HMC5883::collect() /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); - new_report.error_count = perf_event_count(_comms_errors); + new_report.error_count = perf_event_count(_comms_errors); /* * @note We could read the status register here, which could tell us that @@ -886,7 +911,7 @@ HMC5883::collect() if (ret != OK) { perf_count(_comms_errors); - debug("data/status read error"); + DEVICE_DEBUG("data/status read error"); goto out; } @@ -908,6 +933,7 @@ HMC5883::collect() /* get measurements from the device */ new_report.temperature = 0; + if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) { /* if temperature compensation is enabled read the @@ -923,23 +949,27 @@ HMC5883::collect() ret = _interface->read(ADDR_TEMP_OUT_MSB, raw_temperature, sizeof(raw_temperature)); + if (ret == OK) { int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + - raw_temperature[1]; - new_report.temperature = 25 + (temp16 / (16*8.0f)); + raw_temperature[1]; + new_report.temperature = 25 + (temp16 / (16 * 8.0f)); _temperature_error_count = 0; + } else { _temperature_error_count++; + if (_temperature_error_count == 10) { /* it probably really is an old HMC5883, and can't do temperature. Disable it */ _temperature_error_count = 0; - debug("disabling temperature compensation"); + DEVICE_DEBUG("disabling temperature compensation"); set_temperature_compensation(0); } } + } else { new_report.temperature = _last_report.temperature; } @@ -961,17 +991,18 @@ HMC5883::collect() // XXX revisit for SPI part, might require a bus type IOCTL unsigned dummy; sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); + if (sensor_is_onboard) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; report.x = -report.x; - } + } - /* the standard external mag by 3DR has x pointing to the + /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x * and y and invert y */ - xraw_f = -report.y; + xraw_f = -report.y; yraw_f = report.x; zraw_f = report.z; @@ -989,12 +1020,14 @@ HMC5883::collect() if (_mag_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } else { _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report, - &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); + &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); - if (_mag_topic == nullptr) - debug("ADVERT FAIL"); + if (_mag_topic == nullptr) { + DEVICE_DEBUG("ADVERT FAIL"); + } } } @@ -1016,9 +1049,11 @@ HMC5883::collect() vehicles have it is worth checking for. */ check_counter = perf_event_count(_sample_perf) % 256; + if (check_counter == 0) { check_range(); } + if (check_counter == 128) { check_conf(); } @@ -1075,7 +1110,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) } /* Set to 2.5 Gauss. We ask for 3 to get the right part of - * the chained if statement above. */ + * the chained if statement above. */ if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { warnx("FAILED: MAGIOCSRANGE 3.3 Ga"); ret = 1; @@ -1124,8 +1159,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) } } - /* read the sensor up to 50x, stopping when we have 10 good values */ - for (uint8_t i = 0; i < 50 && good_count < 10; i++) { + /* read the sensor up to 100x, stopping when we have 30 good values */ + for (uint8_t i = 0; i < 100 && good_count < 30; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -1146,9 +1181,11 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = -EIO; goto out; } + float cal[3] = {fabsf(expected_cal[0] / report.x), fabsf(expected_cal[1] / report.y), - fabsf(expected_cal[2] / report.z)}; + fabsf(expected_cal[2] / report.z) + }; if (cal[0] > 0.7f && cal[0] < 1.35f && cal[1] > 0.7f && cal[1] < 1.35f && @@ -1172,9 +1209,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) scaling[2] = sum_excited[2] / good_count; /* set scaling in device */ - mscale_previous.x_scale = scaling[0]; - mscale_previous.y_scale = scaling[1]; - mscale_previous.z_scale = scaling[2]; + mscale_previous.x_scale = 1.0f / scaling[0]; + mscale_previous.y_scale = 1.0f / scaling[1]; + mscale_previous.z_scale = 1.0f / scaling[2]; ret = OK; @@ -1211,10 +1248,11 @@ int HMC5883::check_scale() bool scale_valid; if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) && - (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && - (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { + (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && + (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { /* scale is one */ scale_valid = false; + } else { scale_valid = true; } @@ -1228,10 +1266,11 @@ int HMC5883::check_offset() bool offset_valid; if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) && - (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && - (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { + (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && + (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { /* offset is zero */ offset_valid = false; + } else { offset_valid = true; } @@ -1247,7 +1286,7 @@ int HMC5883::check_calibration() if (_calibrated != (offset_valid && scale_valid)) { warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", - (offset_valid) ? "" : "offset invalid"); + (offset_valid) ? "" : "offset invalid"); _calibrated = (offset_valid && scale_valid); } @@ -1261,10 +1300,12 @@ int HMC5883::set_excitement(unsigned enable) /* arm the excitement strap */ ret = read_reg(ADDR_CONF_A, _conf_reg); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } _conf_reg &= ~0x03; // reset previous excitement mode + if (((int)enable) < 0) { _conf_reg |= 0x01; @@ -1273,12 +1314,13 @@ int HMC5883::set_excitement(unsigned enable) } - // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); ret = write_reg(ADDR_CONF_A, _conf_reg); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } uint8_t conf_reg_ret = 0; read_reg(ADDR_CONF_A, conf_reg_ret); @@ -1317,11 +1359,12 @@ int HMC5883::set_temperature_compensation(unsigned enable) if (OK != ret) { perf_count(_comms_errors); - return -EIO; - } + return -EIO; + } if (enable != 0) { _conf_reg |= HMC5983_TEMP_SENSOR_ENABLE; + } else { _conf_reg &= ~HMC5983_TEMP_SENSOR_ENABLE; } @@ -1334,6 +1377,7 @@ int HMC5883::set_temperature_compensation(unsigned enable) } uint8_t conf_reg_ret = 0; + if (read_reg(ADDR_CONF_A, conf_reg_ret) != OK) { perf_count(_comms_errors); return -EIO; @@ -1383,7 +1427,7 @@ HMC5883::print_info() printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, - (double)(1.0f/_range_scale), (double)_range_ga); + (double)(1.0f / _range_scale), (double)_range_ga); printf("temperature %.2f\n", (double)_last_report.temperature); _reports->print_info("report queue"); } @@ -1436,16 +1480,20 @@ void usage(); bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) { - if (bus.dev != nullptr) - errx(1,"bus option already started"); + if (bus.dev != nullptr) { + errx(1, "bus option already started"); + } device::Device *interface = bus.interface_constructor(bus.busnum); + if (interface->init() != OK) { delete interface; warnx("no device on bus %u", (unsigned)bus.busid); return false; } + bus.dev = new HMC5883(interface, bus.devpath, rotation); + if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; bus.dev = NULL; @@ -1453,14 +1501,16 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) } int fd = open(bus.devpath, O_RDONLY); + if (fd < 0) { return false; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { close(fd); - errx(1,"Failed to setup poll rate"); + errx(1, "Failed to setup poll rate"); } + close(fd); return true; @@ -1483,15 +1533,18 @@ start(enum HMC5883_BUS busid, enum Rotation rotation) // this device is already started continue; } + if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) { // not the one that is asked for continue; } + started |= start_bus(bus_options[i], rotation); } - if (!started) - errx(1, "driver start failed"); + if (!started) { + exit(1); + } } /** @@ -1505,6 +1558,7 @@ struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid) return bus_options[i]; } } + errx(1, "bus %u not started", (unsigned)busid); } @@ -1525,31 +1579,37 @@ test(enum HMC5883_BUS busid) int fd = open(path, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'hmc5883 start')", path); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); /* check if mag is onboard or external */ - if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) + if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) { errx(1, "failed to get if mag is onboard or external"); + } + warnx("device active: %s", ret ? "external" : "onboard"); /* set the queue depth to 5 */ - if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { errx(1, "failed to set queue depth"); + } /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -1560,14 +1620,16 @@ test(enum HMC5883_BUS busid) 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("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); @@ -1627,8 +1689,9 @@ int calibrate(enum HMC5883_BUS busid) int fd = open(path, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); + } if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1650,14 +1713,17 @@ reset(enum HMC5883_BUS busid) int fd = open(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); } @@ -1674,11 +1740,13 @@ temp_enable(enum HMC5883_BUS busid, bool enable) int fd = open(path, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) + if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) { err(1, "set temperature compensation failed"); + } close(fd); return 0; @@ -1718,7 +1786,7 @@ hmc5883_main(int argc, char *argv[]) int ch; enum HMC5883_BUS busid = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; - bool calibrate = false; + bool calibrate = false; bool temp_compensation = false; while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) { @@ -1727,22 +1795,28 @@ hmc5883_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) + case 'I': busid = HMC5883_BUS_I2C_INTERNAL; break; #endif + case 'X': busid = HMC5883_BUS_I2C_EXTERNAL; break; + case 'S': busid = HMC5883_BUS_SPI; break; + case 'C': calibrate = true; break; + case 'T': temp_compensation = true; break; + default: hmc5883::usage(); exit(0); @@ -1756,42 +1830,51 @@ hmc5883_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { hmc5883::start(busid, rotation); + if (calibrate && hmc5883::calibrate(busid) != 0) { errx(1, "calibration failed"); } + if (temp_compensation) { // we consider failing to setup temperature // compensation as non-fatal hmc5883::temp_enable(busid, true); } + exit(0); } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { hmc5883::test(busid); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { hmc5883::reset(busid); + } /* * enable/disable temperature compensation */ - if (!strcmp(verb, "tempoff")) + if (!strcmp(verb, "tempoff")) { hmc5883::temp_enable(busid, false); - if (!strcmp(verb, "tempon")) + } + + if (!strcmp(verb, "tempon")) { hmc5883::temp_enable(busid, true); + } /* * Print driver information. */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { hmc5883::info(busid); + } /* * Autocalibrate the scaling diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h index 9607fe61493f..4cef0aa8cfbc 100644 --- a/src/drivers/hmc5883/hmc5883.h +++ b/src/drivers/hmc5883/hmc5883.h @@ -48,6 +48,6 @@ #define ID_C_WHO_AM_I '3' /* interface factories */ -extern device::Device *HMC5883_SPI_interface(int bus) weak_function; -extern device::Device *HMC5883_I2C_interface(int bus) weak_function; +extern device::Device *HMC5883_SPI_interface(int bus); +extern device::Device *HMC5883_I2C_interface(int bus); typedef device::Device *(*HMC5883_constructor)(int); diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index ec7524189af8..d29ffbe0b435 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file HMC5883_I2C.cpp - * - * I2C interface for HMC5883 / HMC 5983 - */ +/** + * @file HMC5883_I2C.cpp + * + * I2C interface for HMC5883 / HMC 5983 + */ /* XXX trim includes */ #include @@ -72,7 +72,7 @@ class HMC5883_I2C : public device::I2C virtual ~HMC5883_I2C(); virtual int init(); - virtual int read(unsigned address, void *data, unsigned count); + virtual int read(unsigned address, void *data, unsigned count); virtual int write(unsigned address, void *data, unsigned count); virtual int ioctl(unsigned operation, unsigned &arg); @@ -89,7 +89,7 @@ HMC5883_I2C_interface(int bus) } HMC5883_I2C::HMC5883_I2C(int bus) : - I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 100000) + I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } @@ -118,11 +118,13 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 if (_bus == PX4_I2C_BUS_EXPANSION) { return 1; + } else { return 0; } + #else - return 1; + return 1; #endif case DEVIOCGDEVICEID: @@ -145,7 +147,7 @@ HMC5883_I2C::probe() if (read(ADDR_ID_A, &data[0], 1) || read(ADDR_ID_B, &data[1], 1) || read(ADDR_ID_C, &data[2], 1)) { - debug("read_reg fail"); + DEVICE_DEBUG("read_reg fail"); return -EIO; } @@ -154,7 +156,7 @@ HMC5883_I2C::probe() if ((data[0] != ID_A_WHO_AM_I) || (data[1] != ID_B_WHO_AM_I) || (data[2] != ID_C_WHO_AM_I)) { - debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + DEVICE_DEBUG("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); return -EIO; } diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp index 0eec3d8d18c5..2710f358029f 100644 --- a/src/drivers/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file HMC5883_SPI.cpp - * - * SPI interface for HMC5983 - */ +/** + * @file HMC5883_SPI.cpp + * + * SPI interface for HMC5983 + */ /* XXX trim includes */ #include @@ -77,7 +77,7 @@ class HMC5883_SPI : public device::SPI virtual ~HMC5883_SPI(); virtual int init(); - virtual int read(unsigned address, void *data, unsigned count); + virtual int read(unsigned address, void *data, unsigned count); virtual int write(unsigned address, void *data, unsigned count); virtual int ioctl(unsigned operation, unsigned &arg); @@ -91,7 +91,7 @@ HMC5883_SPI_interface(int bus) } HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) : - SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */) + SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } @@ -106,8 +106,9 @@ HMC5883_SPI::init() int ret; ret = SPI::init(); + if (ret != OK) { - debug("SPI init failed"); + DEVICE_DEBUG("SPI init failed"); return -EIO; } @@ -117,13 +118,13 @@ HMC5883_SPI::init() if (read(ADDR_ID_A, &data[0], 1) || read(ADDR_ID_B, &data[1], 1) || read(ADDR_ID_C, &data[2], 1)) { - debug("read_reg fail"); + DEVICE_DEBUG("read_reg fail"); } if ((data[0] != ID_A_WHO_AM_I) || (data[1] != ID_B_WHO_AM_I) || (data[2] != ID_C_WHO_AM_I)) { - debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + DEVICE_DEBUG("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); return -EIO; } @@ -148,10 +149,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); - default: - { - ret = -EINVAL; - } + default: { + ret = -EINVAL; + } } return ret; From 388425a1744b4b8c6c5cda812dfb639e8c614533 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:03:54 +1100 Subject: [PATCH 207/293] hott: update from upstream --- src/drivers/hott/CMakeLists.txt | 43 ++++++++ src/drivers/hott/comms.cpp | 7 +- src/drivers/hott/hott_sensors/CMakeLists.txt | 43 ++++++++ .../hott/hott_sensors/hott_sensors.cpp | 21 ++-- .../hott/hott_telemetry/CMakeLists.txt | 43 ++++++++ .../hott/hott_telemetry/hott_telemetry.cpp | 19 +++- src/drivers/hott/messages.cpp | 38 ++++--- src/drivers/hott/messages.h | 104 +++++++++--------- 8 files changed, 233 insertions(+), 85 deletions(-) create mode 100644 src/drivers/hott/CMakeLists.txt create mode 100644 src/drivers/hott/hott_sensors/CMakeLists.txt create mode 100644 src/drivers/hott/hott_telemetry/CMakeLists.txt diff --git a/src/drivers/hott/CMakeLists.txt b/src/drivers/hott/CMakeLists.txt new file mode 100644 index 000000000000..827c72031787 --- /dev/null +++ b/src/drivers/hott/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__hott + COMPILE_FLAGS + -Os + SRCS + messages.cpp + comms.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp index 60a49b559c23..378b5b75d6fe 100644 --- a/src/drivers/hott/comms.cpp +++ b/src/drivers/hott/comms.cpp @@ -57,10 +57,11 @@ open_uart(const char *device) if (uart < 0) { err(1, "ERR: opening %s", device); } - - /* Back up the original uart configuration to restore it after exit */ + + /* Back up the original uart configuration to restore it after exit */ int termios_state; struct termios uart_config_original; + if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { close(uart); err(1, "ERR: %s: %d", device, termios_state); @@ -77,7 +78,7 @@ open_uart(const char *device) if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { close(uart); err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)", - device, termios_state); + device, termios_state); } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { diff --git a/src/drivers/hott/hott_sensors/CMakeLists.txt b/src/drivers/hott/hott_sensors/CMakeLists.txt new file mode 100644 index 000000000000..6d9b9b6c47f2 --- /dev/null +++ b/src/drivers/hott/hott_sensors/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__hott__hott_sensors + MAIN hott_sensors + COMPILE_FLAGS + -Os + SRCS + hott_sensors.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 29680a279f1e..f5c947d4f66c 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -104,15 +104,16 @@ int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) { static const int timeout_ms = 1000; - + struct pollfd fds; fds.fd = uart; fds.events = POLLIN; - + // XXX should this poll be inside the while loop??? if (poll(&fds, 1, timeout_ms) > 0) { int i = 0; bool stop_byte_read = false; + while (true) { read(uart, &buffer[i], sizeof(buffer[i])); @@ -121,14 +122,17 @@ recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) *size = ++i; return OK; } + // XXX can some other field not have the STOP BYTE value? if (buffer[i] == STOP_BYTE) { *id = buffer[1]; stop_byte_read = true; } + i++; } } + return ERROR; } @@ -156,6 +160,7 @@ hott_sensors_thread_main(int argc, char *argv[]) /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ const int uart = open_uart(device); + if (uart < 0) { errx(1, "Open fail, exiting."); thread_running = false; @@ -166,6 +171,7 @@ hott_sensors_thread_main(int argc, char *argv[]) uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE]; size_t size = 0; uint8_t id = 0; + while (!thread_should_exit) { // Currently we only support a General Air Module sensor. build_gam_request(&buffer[0], &size); @@ -179,6 +185,7 @@ hott_sensors_thread_main(int argc, char *argv[]) // Determine which moduel sent it and process accordingly. if (id == GAM_SENSOR_ID) { publish_gam_message(buffer); + } else { warnx("Unknown sensor ID: %d", id); } @@ -210,11 +217,11 @@ hott_sensors_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd(daemon_name, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - hott_sensors_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + hott_sensors_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/drivers/hott/hott_telemetry/CMakeLists.txt b/src/drivers/hott/hott_telemetry/CMakeLists.txt new file mode 100644 index 000000000000..b86ef91cb395 --- /dev/null +++ b/src/drivers/hott/hott_telemetry/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__hott__hott_telemetry + MAIN hott_telemetry + COMPILE_FLAGS + -Os + SRCS + hott_telemetry.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index a1a5b080c55f..f5aa0ec58a5e 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -90,7 +90,7 @@ recv_req_id(int uart, uint8_t *id) static const int timeout_ms = 1000; // TODO make it a define uint8_t mode; - + struct pollfd fds; fds.fd = uart; fds.events = POLLIN; @@ -106,6 +106,7 @@ recv_req_id(int uart, uint8_t *id) /* Read the device ID being polled */ read(uart, id, sizeof(*id)); + } else { warnx("UART timeout on TX/RX port"); return ERROR; @@ -120,6 +121,7 @@ send_data(int uart, uint8_t *buffer, size_t size) usleep(POST_READ_DELAY_IN_USECS); uint16_t checksum = 0; + for (size_t i = 0; i < size; i++) { if (i == size - 1) { /* Set the checksum: the first uint8_t is taken as the checksum. */ @@ -167,6 +169,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ const int uart = open_uart(device); + if (uart < 0) { errx(1, "Failed opening HoTT UART, exiting."); thread_running = false; @@ -178,6 +181,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) size_t size = 0; uint8_t id = 0; bool connected = true; + while (!thread_should_exit) { // Listen for and serve poll from the receiver. if (recv_req_id(uart, &id) == OK) { @@ -190,9 +194,11 @@ hott_telemetry_thread_main(int argc, char *argv[]) case EAM_SENSOR_ID: build_eam_response(buffer, &size); break; + case GAM_SENSOR_ID: build_gam_response(buffer, &size); break; + case GPS_SENSOR_ID: build_gps_response(buffer, &size); break; @@ -202,6 +208,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) } send_data(uart, buffer, size); + } else { connected = false; warnx("syncing"); @@ -236,11 +243,11 @@ hott_telemetry_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd(daemon_name, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - hott_telemetry_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + hott_telemetry_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index ef841c3f852a..db225a2b98e6 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -69,7 +69,7 @@ static bool _home_position_set = false; static double _home_lat = 0.0d; static double _home_lon = 0.0d; -void +void init_sub_messages(void) { _battery_sub = orb_subscribe(ORB_ID(battery_status)); @@ -80,7 +80,7 @@ init_sub_messages(void) _esc_sub = orb_subscribe(ORB_ID(esc_status)); } -void +void init_pub_messages(void) { } @@ -122,12 +122,13 @@ publish_gam_message(const uint8_t *buffer) /* announce the esc if needed, just publish else */ if (_esc_pub != nullptr) { orb_publish(ORB_ID(esc_status), _esc_pub, &esc); + } else { _esc_pub = orb_advertise(ORB_ID(esc_status), &esc); } } -void +void build_eam_response(uint8_t *buffer, size_t *size) { /* get a local copy of the current sensor values */ @@ -147,13 +148,13 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.start = START_BYTE; msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - - msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); + + msg.temperature1 = (uint8_t)(raw.baro_temp_celcius[0] + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); - uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); + uint16_t alt = (uint16_t)(raw.baro_alt_meter[0] + 500); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; @@ -170,7 +171,7 @@ build_eam_response(uint8_t *buffer, size_t *size) memcpy(buffer, &msg, *size); } -void +void build_gam_response(uint8_t *buffer, size_t *size) { /* get a local copy of the ESC Status values */ @@ -185,7 +186,7 @@ build_gam_response(uint8_t *buffer, size_t *size) msg.start = START_BYTE; msg.gam_sensor_id = GAM_SENSOR_ID; msg.sensor_text_id = GAM_SENSOR_TEXT_ID; - + msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F); msg.temperature2 = 20; // 0 deg. C. @@ -205,7 +206,7 @@ build_gam_response(uint8_t *buffer, size_t *size) memcpy(buffer, &msg, *size); } -void +void build_gps_response(uint8_t *buffer, size_t *size) { /* get a local copy of the current sensor values */ @@ -213,7 +214,7 @@ build_gps_response(uint8_t *buffer, size_t *size) memset(&raw, 0, sizeof(raw)); orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); - /* get a local copy of the battery data */ + /* get a local copy of the battery data */ struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); @@ -241,12 +242,13 @@ build_gps_response(uint8_t *buffer, size_t *size) uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f); msg.gps_speed_L = (uint8_t)speed & 0xff; msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; - + /* Get latitude in degrees, minutes and seconds */ - double lat = ((double)(gps.lat))*1e-7d; + double lat = ((double)(gps.lat)) * 1e-7d; - /* Set the N or S specifier */ + /* Set the N or S specifier */ msg.latitude_ns = 0; + if (lat < 0) { msg.latitude_ns = 1; lat = abs(lat); @@ -265,32 +267,34 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff; /* Get longitude in degrees, minutes and seconds */ - double lon = ((double)(gps.lon))*1e-7d; + double lon = ((double)(gps.lon)) * 1e-7d; /* Set the E or W specifier */ msg.longitude_ew = 0; + if (lon < 0) { msg.longitude_ew = 1; lon = abs(lon); } convert_to_degrees_minutes_seconds(lon, °, &min, &sec); - + uint16_t lon_min = (uint16_t)(deg * 100 + min); msg.longitude_min_L = (uint8_t)lon_min & 0xff; msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff; uint16_t lon_sec = (uint16_t)(sec); msg.longitude_sec_L = (uint8_t)lon_sec & 0xff; msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; - + /* Altitude */ - uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f); + uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; /* Get any (and probably only ever one) _home_sub postion report */ bool updated; orb_check(_home_sub, &updated); + if (updated) { /* get a local copy of the home position data */ struct home_position_s home; diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h index a116a50dd6a0..504c9f19ec85 100644 --- a/src/drivers/hott/messages.h +++ b/src/drivers/hott/messages.h @@ -125,63 +125,63 @@ struct eam_module_msg { #define GAM_SENSOR_TEXT_ID 0xd0 struct gam_module_msg { - uint8_t start; /**< Start byte */ - uint8_t gam_sensor_id; /**< GAM sensor id */ - uint8_t warning_beeps; - uint8_t sensor_text_id; - uint8_t alarm_invers1; - uint8_t alarm_invers2; - uint8_t cell1; /**< Lipo cell voltages. Not supported. */ - uint8_t cell2; - uint8_t cell3; - uint8_t cell4; - uint8_t cell5; - uint8_t cell6; - uint8_t batt1_L; /**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V */ - uint8_t batt1_H; - uint8_t batt2_L; /**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V */ - uint8_t batt2_H; - uint8_t temperature1; /**< Temperature 1. offset of 20. a value of 20 = 0°C */ - uint8_t temperature2; /**< Temperature 2. offset of 20. a value of 20 = 0°C */ - uint8_t fuel_procent; /**< Fuel capacity in %. Values 0 - 100 */ - /**< Graphical display ranges: 0 25% 50% 75% 100% */ - uint8_t fuel_ml_L; /**< Fuel in ml scale. Full = 65535 */ - uint8_t fuel_ml_H; - uint8_t rpm_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ - uint8_t rpm_H; - uint8_t altitude_L; /**< Altitude in meters. offset of 500, 500 = 0m */ - uint8_t altitude_H; - uint8_t climbrate_L; /**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */ - uint8_t climbrate_H; - uint8_t climbrate3s; /**< Climb rate in m/3sec. Value of 120 = 0m/3sec */ - uint8_t current_L; /**< Current in 0.1A steps */ - uint8_t current_H; - uint8_t main_voltage_L; /**< Main power voltage using 0.1V steps */ - uint8_t main_voltage_H; - uint8_t batt_cap_L; /**< Used battery capacity in 10mAh steps */ - uint8_t batt_cap_H; - uint8_t speed_L; /**< Speed in km/h */ - uint8_t speed_H; - uint8_t min_cell_volt; /**< Minimum cell voltage in 2mV steps. 124 = 2,48V */ - uint8_t min_cell_volt_num; /**< Number of the cell with the lowest voltage */ - uint8_t rpm2_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ - uint8_t rpm2_H; - uint8_t general_error_number; /**< Voice error == 12. TODO: more docu */ - uint8_t pressure; /**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */ - uint8_t version; - uint8_t stop; /**< Stop byte */ - uint8_t checksum; /**< Lower 8-bits of all bytes summed */ + uint8_t start; /**< Start byte */ + uint8_t gam_sensor_id; /**< GAM sensor id */ + uint8_t warning_beeps; + uint8_t sensor_text_id; + uint8_t alarm_invers1; + uint8_t alarm_invers2; + uint8_t cell1; /**< Lipo cell voltages. Not supported. */ + uint8_t cell2; + uint8_t cell3; + uint8_t cell4; + uint8_t cell5; + uint8_t cell6; + uint8_t batt1_L; /**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V */ + uint8_t batt1_H; + uint8_t batt2_L; /**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V */ + uint8_t batt2_H; + uint8_t temperature1; /**< Temperature 1. offset of 20. a value of 20 = 0°C */ + uint8_t temperature2; /**< Temperature 2. offset of 20. a value of 20 = 0°C */ + uint8_t fuel_procent; /**< Fuel capacity in %. Values 0 - 100 */ + /**< Graphical display ranges: 0 25% 50% 75% 100% */ + uint8_t fuel_ml_L; /**< Fuel in ml scale. Full = 65535 */ + uint8_t fuel_ml_H; + uint8_t rpm_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ + uint8_t rpm_H; + uint8_t altitude_L; /**< Altitude in meters. offset of 500, 500 = 0m */ + uint8_t altitude_H; + uint8_t climbrate_L; /**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */ + uint8_t climbrate_H; + uint8_t climbrate3s; /**< Climb rate in m/3sec. Value of 120 = 0m/3sec */ + uint8_t current_L; /**< Current in 0.1A steps */ + uint8_t current_H; + uint8_t main_voltage_L; /**< Main power voltage using 0.1V steps */ + uint8_t main_voltage_H; + uint8_t batt_cap_L; /**< Used battery capacity in 10mAh steps */ + uint8_t batt_cap_H; + uint8_t speed_L; /**< Speed in km/h */ + uint8_t speed_H; + uint8_t min_cell_volt; /**< Minimum cell voltage in 2mV steps. 124 = 2,48V */ + uint8_t min_cell_volt_num; /**< Number of the cell with the lowest voltage */ + uint8_t rpm2_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ + uint8_t rpm2_H; + uint8_t general_error_number; /**< Voice error == 12. TODO: more docu */ + uint8_t pressure; /**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */ + uint8_t version; + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed */ }; /* GPS sensor constants. */ #define GPS_SENSOR_ID 0x8a #define GPS_SENSOR_TEXT_ID 0xa0 -/** +/** * The GPS sensor message - * Struct based on: https://code.google.com/p/diy-hott-gps/downloads + * Struct based on: https://code.google.com/p/diy-hott-gps/downloads */ -struct gps_module_msg { +struct gps_module_msg { uint8_t start; /**< Start byte */ uint8_t sensor_id; /**< GPS sensor ID*/ uint8_t warning; /**< 0…= warning beeps */ @@ -191,19 +191,19 @@ struct gps_module_msg { uint8_t flight_direction; /**< 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */ uint8_t gps_speed_L; /**< 8 = /GPS speed low byte 8km/h */ uint8_t gps_speed_H; /**< 0 = /GPS speed high byte */ - + uint8_t latitude_ns; /**< 000 = N = 48°39’988 */ uint8_t latitude_min_L; /**< 231 0xE7 = 0x12E7 = 4839 */ uint8_t latitude_min_H; /**< 018 18 = 0x12 */ uint8_t latitude_sec_L; /**< 171 220 = 0xDC = 0x03DC =0988 */ uint8_t latitude_sec_H; /**< 016 3 = 0x03 */ - + uint8_t longitude_ew; /**< 000 = E= 9° 25’9360 */ uint8_t longitude_min_L; /**< 150 157 = 0x9D = 0x039D = 0925 */ uint8_t longitude_min_H; /**< 003 3 = 0x03 */ uint8_t longitude_sec_L; /**< 056 144 = 0x90 0x2490 = 9360 */ uint8_t longitude_sec_H; /**< 004 36 = 0x24 */ - + uint8_t distance_L; /**< 027 123 = /distance low byte 6 = 6 m */ uint8_t distance_H; /**< 036 35 = /distance high byte */ uint8_t altitude_L; /**< 243 244 = /Altitude low byte 500 = 0m */ From 26de30e95d64f015105370e7924e11955629b076 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:04:02 +1100 Subject: [PATCH 208/293] irlock: update from upstream --- src/drivers/irlock/CMakeLists.txt | 43 +++++++++++++++++++++++++++++++ src/drivers/irlock/irlock.cpp | 4 +-- 2 files changed, 45 insertions(+), 2 deletions(-) create mode 100644 src/drivers/irlock/CMakeLists.txt diff --git a/src/drivers/irlock/CMakeLists.txt b/src/drivers/irlock/CMakeLists.txt new file mode 100644 index 000000000000..d821afb3b672 --- /dev/null +++ b/src/drivers/irlock/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__irlock + MAIN irlock + COMPILE_FLAGS + -Os + SRCS + irlock.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 19b0de6d5fea..8be2f8429b8d 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -388,8 +388,8 @@ int IRLOCK::read_device_block(struct irlock_s *block) /** convert to angles **/ block->target_num = target_num; - block->angle_x = (((float)(pixel_x-IRLOCK_CENTER_X))/IRLOCK_PIXELS_PER_RADIAN_X); - block->angle_y = (((float)(pixel_y-IRLOCK_CENTER_Y))/IRLOCK_PIXELS_PER_RADIAN_Y); + block->angle_x = (((float)(pixel_x - IRLOCK_CENTER_X)) / IRLOCK_PIXELS_PER_RADIAN_X); + block->angle_y = (((float)(pixel_y - IRLOCK_CENTER_Y)) / IRLOCK_PIXELS_PER_RADIAN_Y); block->size_x = pixel_size_x / IRLOCK_PIXELS_PER_RADIAN_X; block->size_y = pixel_size_y / IRLOCK_PIXELS_PER_RADIAN_Y; From 7ff8a2e6a8bf75553f9d9902fec8c6b1eddf79d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:04:08 +1100 Subject: [PATCH 209/293] led: update from upstream --- src/drivers/led/CMakeLists.txt | 42 ++++++++++++++++++++++++++++++++++ src/drivers/led/led.cpp | 8 ++++--- 2 files changed, 47 insertions(+), 3 deletions(-) create mode 100644 src/drivers/led/CMakeLists.txt diff --git a/src/drivers/led/CMakeLists.txt b/src/drivers/led/CMakeLists.txt new file mode 100644 index 000000000000..2279751d8ce0 --- /dev/null +++ b/src/drivers/led/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__led + COMPILE_FLAGS + -Os + SRCS + led.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 69c72b5b539e..f74253fa37e6 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -38,7 +38,6 @@ */ #include -#include #include #include #include @@ -89,7 +88,7 @@ LED::~LED() int LED::init() { - debug("LED::init"); + DEVICE_DEBUG("LED::init"); #ifdef __PX4_NUTTX CDev::init(); #else @@ -126,6 +125,7 @@ LED::ioctl(device::file_t *filp, int cmd, unsigned long arg) result = VDev::ioctl(filp, cmd, arg); #endif } + return result; } @@ -139,7 +139,9 @@ drv_led_start(void) { if (gLED == nullptr) { gLED = new LED; - if (gLED != nullptr) + + if (gLED != nullptr) { gLED->init(); + } } } From 7764a1dc21e5522f77a2e1298382108bd5818905 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:04:26 +1100 Subject: [PATCH 210/293] ll40ls: update for debug macros --- src/drivers/ll40ls/LidarLiteI2C.cpp | 16 ++++++++-------- src/drivers/ll40ls/LidarLitePWM.cpp | 4 ++-- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 964ad978ce28..d65d8243235e 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -138,7 +138,7 @@ int LidarLiteI2C::init() &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { - debug("failed to create distance_sensor object. Did you start uOrb?"); + DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -195,9 +195,9 @@ int LidarLiteI2C::probe() goto ok; } - debug("probe failed hw_version=0x%02x sw_version=0x%02x\n", - (unsigned)_hw_version, - (unsigned)_sw_version); + DEVICE_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n", + (unsigned)_hw_version, + (unsigned)_sw_version); } // not found on any address @@ -341,7 +341,7 @@ int LidarLiteI2C::measure() if (OK != ret) { perf_count(_comms_errors); - debug("i2c::transfer returned %d", ret); + DEVICE_DEBUG("i2c::transfer returned %d", ret); // if we are getting lots of I2C transfer errors try // resetting the sensor @@ -496,7 +496,7 @@ int LidarLiteI2C::collect() read before it is ready, so only consider it an error if more than 100ms has elapsed. */ - debug("error reading from sensor: %d", ret); + DEVICE_DEBUG("error reading from sensor: %d", ret); perf_count(_comms_errors); if (perf_event_count(_comms_errors) % 10 == 0) { @@ -611,7 +611,7 @@ void LidarLiteI2C::cycle() /* try a collection */ if (OK != collect()) { - debug("collection error"); + DEVICE_DEBUG("collection error"); /* if we've been waiting more than 200ms then send a new acquire */ @@ -643,7 +643,7 @@ void LidarLiteI2C::cycle() if (_collect_phase == false) { /* measurement phase */ if (OK != measure()) { - debug("measure error"); + DEVICE_DEBUG("measure error"); } else { /* next phase is collection. Don't switch to diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 406aa111b855..3d17d5b36be8 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -117,7 +117,7 @@ int LidarLitePWM::init() &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { - debug("failed to create distance_sensor object. Did you start uOrb?"); + DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -175,7 +175,7 @@ int LidarLitePWM::measure() perf_begin(_sample_perf); if (OK != collect()) { - debug("collection error"); + DEVICE_DEBUG("collection error"); perf_count(_read_errors); perf_end(_sample_perf); return ERROR; From f134295917f124c757a67e06499ee1e63f2a5c51 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:04:40 +1100 Subject: [PATCH 211/293] mb12xx: update from upstream --- src/drivers/mb12xx/CMakeLists.txt | 43 +++++++++++++++++++++++++++++++ src/drivers/mb12xx/mb12xx.cpp | 41 +++++++++++++++-------------- 2 files changed, 65 insertions(+), 19 deletions(-) create mode 100644 src/drivers/mb12xx/CMakeLists.txt diff --git a/src/drivers/mb12xx/CMakeLists.txt b/src/drivers/mb12xx/CMakeLists.txt new file mode 100644 index 000000000000..3517cf4eb01e --- /dev/null +++ b/src/drivers/mb12xx/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__mb12xx + MAIN mb12xx + COMPILE_FLAGS + -Os + SRCS + mb12xx.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index a67db0bd1f42..39f151f1d6f5 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -143,7 +143,8 @@ class MB12XX : public device::I2C int _cycling_rate; /* */ uint8_t _index_counter; /* temporary sonar i2c address */ std::vector addr_ind; /* temp sonar i2c address vector */ - std::vector _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + std::vector + _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ /** @@ -254,7 +255,7 @@ MB12XX::init() /* do I2C init (and probe) first */ if (I2C::init() != OK) { - goto out; + return ret; } /* allocate basic report buffers */ @@ -264,21 +265,19 @@ MB12XX::init() set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ if (_reports == nullptr) { - goto out; + return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report = {}; - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); - if (_distance_sensor_topic == nullptr) { - log("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } // XXX we should find out why we need to wait 200 ms here @@ -294,7 +293,7 @@ MB12XX::init() if (ret2 == 0) { /* sonar is present -> store address_index in array */ addr_ind.push_back(_index_counter); - debug("sonar added"); + DEVICE_DEBUG("sonar added"); _latest_sonar_measurements.push_back(200); } } @@ -312,15 +311,15 @@ MB12XX::init() /* 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]); + DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]); } - debug("Number of sonars connected: %d", addr_ind.size()); + DEVICE_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; -out: + return ret; } @@ -545,7 +544,7 @@ MB12XX::measure() if (OK != ret) { perf_count(_comms_errors); - debug("i2c::transfer returned %d", ret); + DEVICE_DEBUG("i2c::transfer returned %d", ret); return ret; } @@ -567,7 +566,7 @@ MB12XX::collect() ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { - debug("error reading from sensor: %d", ret); + DEVICE_DEBUG("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -660,7 +659,7 @@ MB12XX::cycle() /* perform collection */ if (OK != collect()) { - debug("collection error"); + DEVICE_DEBUG("collection error"); /* if error restart the measurement state machine */ start(); return; @@ -699,7 +698,7 @@ MB12XX::cycle() /* Perform measurement */ if (OK != measure()) { - debug("measure error sonar adress %d", _index_counter); + DEVICE_DEBUG("measure error sonar adress %d", _index_counter); } /* next phase is collection */ @@ -832,6 +831,7 @@ test() } warnx("single read"); + warnx("measurement: %0.2f m", (double)report.current_distance); warnx("time: %llu", report.timestamp); /* start the sensor polling at 2Hz */ @@ -860,6 +860,9 @@ test() } warnx("periodic read %u", i); + warnx("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + warnx("measurement: %0.3f", (double)report.current_distance); warnx("time: %llu", report.timestamp); } From 5e9323dfd4f5618e383f55a91341b45a0716489e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:04:48 +1100 Subject: [PATCH 212/293] md25: update from upstream --- src/drivers/md25/CMakeLists.txt | 44 +++++++++++++++++++++++++++++++++ src/drivers/md25/md25.cpp | 26 ++++++++++--------- src/drivers/md25/md25_main.cpp | 15 +++++------ 3 files changed, 66 insertions(+), 19 deletions(-) create mode 100644 src/drivers/md25/CMakeLists.txt diff --git a/src/drivers/md25/CMakeLists.txt b/src/drivers/md25/CMakeLists.txt new file mode 100644 index 000000000000..d57ff5b4f572 --- /dev/null +++ b/src/drivers/md25/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__md25 + MAIN md25 + COMPILE_FLAGS + -Os + SRCS + md25.cpp + md25_main.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 5d1f58b8549c..041772b4bf67 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -333,7 +333,7 @@ void MD25::update() // check for exit condition every second // note "::poll" is required to distinguish global // poll from member function for driver - if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error + if (::poll(&_controlPoll, 1, 1000) < 0) { return; } // poll error // if new data, send to motors if (_actuators.updated()) { @@ -350,7 +350,7 @@ int MD25::probe() int ret = OK; // try initial address first, if good, then done - if (readData() == OK) return ret; + if (readData() == OK) { return ret; } // try all other addresses uint8_t testAddress = 0; @@ -451,9 +451,9 @@ float MD25::_uint8ToNorm(uint8_t value) uint8_t MD25::_normToUint8(float value) { - if (value > 1) value = 1; + if (value > 1) { value = 1; } - if (value < -1) value = -1; + if (value < -1) { value = -1; } // TODO, should go from 0 to 255 // possibly should handle this differently @@ -494,7 +494,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) break; } - if (t > 2.0f) break; + if (t > 2.0f) { break; } } md25.setMotor1Speed(0); @@ -514,7 +514,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) break; } - if (t > 2.0f) break; + if (t > 2.0f) { break; } } md25.setMotor1Speed(0); @@ -536,7 +536,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) break; } - if (t > 2.0f) break; + if (t > 2.0f) { break; } } md25.setMotor2Speed(0); @@ -556,7 +556,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) break; } - if (t > 2.0f) break; + if (t > 2.0f) { break; } } md25.setMotor2Speed(0); @@ -592,13 +592,14 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu // sine wave for motor 1 md25.resetEncoders(); + while (true) { // input uint64_t timestamp = hrt_absolute_time(); - float t = timestamp/1000000.0f; + float t = timestamp / 1000000.0f; - float input_value = amplitude*sinf(2*M_PI*frequency*t); + float input_value = amplitude * sinf(2 * M_PI * frequency * t); md25.setMotor1Speed(input_value); // output @@ -613,11 +614,11 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu // send output message strncpy(debug_msg.key, "md25 out ", 10); - debug_msg.timestamp_ms = 1000*timestamp; + debug_msg.timestamp_ms = 1000 * timestamp; debug_msg.value = current_revolution; debug_msg.update(); - if (t > t_final) break; + if (t > t_final) { break; } // update for next step prev_revolution = current_revolution; @@ -625,6 +626,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu // sleep usleep(1000000 * dt); } + md25.setMotor1Speed(0); printf("md25 sine complete\n"); diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index a20bf7c7c655..5b889bf03a24 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -79,8 +79,9 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n"); exit(1); @@ -111,11 +112,11 @@ int md25_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd("md25", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 2048, - md25_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + md25_thread_main, + (const char **)argv); exit(0); } @@ -206,7 +207,7 @@ int md25_main(int argc, char *argv[]) exit(0); } - + if (!strcmp(argv[1], "search")) { if (argc < 3) { From 9d66c124ed21ef02222c3496f9d52a071cf90e3b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:04:58 +1100 Subject: [PATCH 213/293] meas_airspeed: update from upstream --- src/drivers/meas_airspeed/CMakeLists.txt | 45 +++++++++++++++++++++ src/drivers/meas_airspeed/meas_airspeed.cpp | 28 +++++++++---- 2 files changed, 66 insertions(+), 7 deletions(-) create mode 100644 src/drivers/meas_airspeed/CMakeLists.txt diff --git a/src/drivers/meas_airspeed/CMakeLists.txt b/src/drivers/meas_airspeed/CMakeLists.txt new file mode 100644 index 000000000000..16e0f97b59ad --- /dev/null +++ b/src/drivers/meas_airspeed/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__meas_airspeed + MAIN meas_airspeed + STACK 1200 + COMPILE_FLAGS + -Weffc++ + -Os + SRCS + meas_airspeed.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index ac3339f38c27..eaf68a3191dc 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -138,7 +138,7 @@ class MEASAirspeed : public Airspeed extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path), + CONVERSION_INTERVAL, path), _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), _t_system_power(-1), system_power{} @@ -189,9 +189,11 @@ MEASAirspeed::collect() break; case 1: - /* fallthrough */ + + /* fallthrough */ case 2: - /* fallthrough */ + + /* fallthrough */ case 3: perf_count(_comms_errors); perf_end(_sample_perf); @@ -219,11 +221,11 @@ MEASAirspeed::collect() are generated when the bottom port is used as the static port on the pitot and top port is used as the dynamic port */ - float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; - // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa_raw, temperature); + // correct for 5V rail voltage if possible + voltage_correction(diff_press_pa_raw, temperature); // the raw value still should be compensated for the known offset diff_press_pa_raw -= _diff_pres_offset; @@ -276,6 +278,7 @@ MEASAirspeed::cycle() /* perform collection */ ret = collect(); + if (OK != ret) { /* restart the measurement state machine */ start(); @@ -304,8 +307,9 @@ MEASAirspeed::cycle() /* measurement phase */ ret = measure(); + if (OK != ret) { - debug("measure error"); + DEVICE_DEBUG("measure error"); } _sensor_ok = (ret == OK); @@ -336,15 +340,19 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); } + if (_t_system_power == -1) { // not available return; } + bool updated = false; orb_check(_t_system_power, &updated); + if (updated) { orb_copy(ORB_ID(system_power), _t_system_power, &system_power); } + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { // not valid, skip correction return; @@ -355,12 +363,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) apply a piecewise linear correction, flattening at 0.5V from 5V */ float voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { voltage_diff = 0.5f; } + if (voltage_diff < -0.5f) { voltage_diff = -0.5f; } + diff_press_pa -= voltage_diff * slope; /* @@ -368,12 +379,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) */ const float temp_slope = 0.887f; voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { voltage_diff = 0.5f; } + if (voltage_diff < -1.0f) { voltage_diff = -1.0f; } + temperature -= voltage_diff * temp_slope; #endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) } From 0a57de2edea3c27e25c10d5e34087cfac754e2af Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:05:08 +1100 Subject: [PATCH 214/293] mkblctrl: update from upstream --- src/drivers/mkblctrl/CMakeLists.txt | 44 +++++++++ src/drivers/mkblctrl/mkblctrl.cpp | 133 ++++++++++++++++------------ 2 files changed, 122 insertions(+), 55 deletions(-) create mode 100644 src/drivers/mkblctrl/CMakeLists.txt diff --git a/src/drivers/mkblctrl/CMakeLists.txt b/src/drivers/mkblctrl/CMakeLists.txt new file mode 100644 index 000000000000..1a711bdfa044 --- /dev/null +++ b/src/drivers/mkblctrl/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__mkblctrl + MAIN mkblctrl + COMPILE_FLAGS + -Os + SRCS + mkblctrl.cpp + mkblctrl_params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3448b570c9d6..fd0ac5955128 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -180,9 +180,9 @@ class MK : public device::I2C void task_main(); static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); int pwm_ioctl(file *filp, int cmd, unsigned long arg); int mk_servo_arm(bool status); @@ -191,7 +191,7 @@ class MK : public device::I2C int mk_servo_locate(); short scaling(float val, float inMin, float inMax, float outMin, float outMax); void play_beep(int count); - + }; @@ -266,8 +266,9 @@ MK::~MK() } /* clean up the alternate device node */ - if (_primary_pwm_device) + if (_primary_pwm_device) { unregister_driver(_device); + } g_mk = nullptr; } @@ -276,7 +277,7 @@ int MK::init(unsigned motors) { _param_indicate_esc = param_find("MKBLCTRL_TEST"); - + _num_outputs = motors; debugCounter = 0; int ret; @@ -295,7 +296,7 @@ MK::init(unsigned motors) ret = register_driver(_device, &fops, 0666, (void *)this); if (ret == OK) { - log("creating alternate output device"); + DEVICE_LOG("creating alternate output device"); _primary_pwm_device = true; } @@ -303,15 +304,15 @@ MK::init(unsigned motors) /* start the IO interface task */ _task = px4_task_spawn_cmd("mkblctrl", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 1500, - (main_t)&MK::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 1500, + (main_t)&MK::task_main_trampoline, + nullptr); if (_task < 0) { - debug("task start failed: %d", errno); + DEVICE_DEBUG("task start failed: %d", errno); return -errno; } @@ -448,7 +449,7 @@ void MK::play_beep(int count) { int buzzer = ::open(TONEALARM0_DEVICE_PATH, O_WRONLY); - + for (int i = 0; i < count; i++) { ::ioctl(buzzer, TONE_SET_ALARM, TONE_SINGLE_BEEP_TUNE); usleep(300000); @@ -467,7 +468,7 @@ MK::task_main() */ _t_actuators = orb_subscribe(ORB_ID(actuator_controls_0)); orb_set_interval(_t_actuators, int(1000 / _update_rate)); /* set the topic update rate (200Hz)*/ - + /* * Subscribe to actuator_armed topic. */ @@ -481,7 +482,7 @@ MK::task_main() memset(&outputs, 0, sizeof(outputs)); int dummy; _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), - &outputs, &dummy, ORB_PRIO_HIGH); + &outputs, &dummy, ORB_PRIO_HIGH); /* * advertise the blctrl status. @@ -499,14 +500,16 @@ MK::task_main() up_pwm_servo_set_rate(_update_rate); /* unnecessary ? */ - log("starting"); + DEVICE_LOG("starting"); /* loop until killed */ while (!_task_should_exit) { - param_get(_param_indicate_esc ,¶m_mkblctrl_test); + param_get(_param_indicate_esc , ¶m_mkblctrl_test); + if (param_mkblctrl_test > 0) { _indicate_esc = true; + } else { _indicate_esc = false; } @@ -516,7 +519,7 @@ MK::task_main() /* this would be bad... */ if (ret < 0) { - log("poll error %d", errno); + DEVICE_LOG("poll error %d", errno); usleep(1000000); continue; } @@ -575,7 +578,8 @@ MK::task_main() /* output to BLCtrl's */ if (_motortest != true && _indicate_esc != true) { Motor[i].SetPoint_PX4 = outputs.output[i]; - mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine + mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, + 2047)); // scale the output to 0 - 2047 and sent to output routine } } } @@ -615,6 +619,7 @@ MK::task_main() if (Motor[i].Version == 1) { // BLCtrl 2.0 (11Bit) esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits; + } else { // BLCtrl < 2.0 (8Bit) esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; @@ -628,7 +633,7 @@ MK::task_main() if (_motortest == true) { mk_servo_test(i); } - + // if esc locate is requested if (_indicate_esc == true) { mk_servo_locate(); @@ -648,7 +653,7 @@ MK::task_main() /* make sure servos are off */ up_pwm_servo_deinit(); - log("stopping"); + DEVICE_LOG("stopping"); /* note - someone else is responsible for restoring the GPIO config */ @@ -719,7 +724,8 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) fprintf(stderr, "[mkblctrl] MotorsFound: %i\n", foundMotorCount); for (unsigned i = 0; i < foundMotorCount; i++) { - fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); + fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, + Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); } @@ -776,14 +782,14 @@ MK::mk_servo_set(unsigned int chan, short val) Motor[chan].Temperature = 255;; } else { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } Motor[chan].RoundCount = 0; } else { if (OK != transfer(&msg[0], 1, nullptr, 0)) { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } } @@ -806,14 +812,14 @@ MK::mk_servo_set(unsigned int chan, short val) Motor[chan].Temperature = result[2]; } else { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } Motor[chan].RoundCount = 0; } else { if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } } @@ -830,7 +836,8 @@ MK::mk_servo_set(unsigned int chan, short val) for (unsigned int i = 0; i < _num_outputs; i++) { if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) { - fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); + fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, + Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); } } @@ -924,12 +931,12 @@ MK::mk_servo_locate() set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); chan++; - + if (chan <= _num_outputs) { fprintf(stderr, "[mkblctrl] ESC Locate - #%i:\tgreen\n", chan); play_beep(chan); } - + if (chan > _num_outputs) { chan = 0; } @@ -962,8 +969,9 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) + if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); + } return ret; } @@ -1064,8 +1072,9 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - if (_mixers == nullptr) + if (_mixers == nullptr) { _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + } if (_mixers == nullptr) { ret = -ENOMEM; @@ -1075,7 +1084,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - debug("mixer load failed with %d", ret); + DEVICE_DEBUG("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; ret = -EINVAL; @@ -1086,30 +1095,36 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) } case PWM_SERVO_SET_MIN_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - if (pwm->channel_count > _max_actuators) - /* fail with error */ - return -E2BIG; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - set_rc_min_value((unsigned)pwm->values[0]); - ret = OK; - break; - } + if (pwm->channel_count > _max_actuators) + /* fail with error */ + { + return -E2BIG; + } + + set_rc_min_value((unsigned)pwm->values[0]); + ret = OK; + break; + } case PWM_SERVO_GET_MIN_PWM: ret = OK; break; case PWM_SERVO_SET_MAX_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - if (pwm->channel_count > _max_actuators) - /* fail with error */ - return -E2BIG; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - set_rc_max_value((unsigned)pwm->values[0]); - ret = OK; - break; - } + if (pwm->channel_count > _max_actuators) + /* fail with error */ + { + return -E2BIG; + } + + set_rc_max_value((unsigned)pwm->values[0]); + ret = OK; + break; + } case PWM_SERVO_GET_MAX_PWM: ret = OK; @@ -1168,7 +1183,8 @@ enum FrameType { int -mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks, unsigned rcmin, unsigned rcmax) +mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks, unsigned rcmin, + unsigned rcmax) { int shouldStop = 0; @@ -1215,8 +1231,9 @@ mk_start(unsigned motors, const char *device_path) // try i2c3 first g_mk = new MK(3, device_path); - if (!g_mk) + if (!g_mk) { return -ENOMEM; + } if (OK == g_mk->init(motors)) { warnx("[mkblctrl] scanning i2c3...\n"); @@ -1233,8 +1250,9 @@ mk_start(unsigned motors, const char *device_path) // fallback to bus 1 g_mk = new MK(1, device_path); - if (!g_mk) + if (!g_mk) { return -ENOMEM; + } if (OK == g_mk->init(motors)) { warnx("[mkblctrl] scanning i2c1...\n"); @@ -1330,13 +1348,15 @@ mkblctrl_main(int argc, char *argv[]) } /* look for the optional -rc_min parameter */ - if (strcmp(argv[i], "-rc_min") == 0 ) { + if (strcmp(argv[i], "-rc_min") == 0) { if (argc > i + 1) { rc_min_value = strtoul(argv[i + 1], &ep, 0); + if (*ep != '\0') { errx(1, "bad pwm val (-rc_min)"); return 1; } + } else { errx(1, "missing value (-rc_min)"); return 1; @@ -1344,13 +1364,15 @@ mkblctrl_main(int argc, char *argv[]) } /* look for the optional -rc_max parameter */ - if (strcmp(argv[i], "-rc_max") == 0 ) { + if (strcmp(argv[i], "-rc_max") == 0) { if (argc > i + 1) { rc_max_value = strtoul(argv[i + 1], &ep, 0); + if (*ep != '\0') { errx(1, "bad pwm val (-rc_max)"); return 1; } + } else { errx(1, "missing value (-rc_max)"); return 1; @@ -1365,7 +1387,8 @@ mkblctrl_main(int argc, char *argv[]) fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n"); fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); - fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); + fprintf(stderr, + "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); fprintf(stderr, "\t -rcmin {pwn-value}\t\t Set RC_MIN Value.\n"); fprintf(stderr, "\t -rcmax {pwn-value}\t\t Set RC_MAX Value.\n"); fprintf(stderr, "\n"); From 1078f56464fdfe8341bd21b4ed5b9d4808b4b05b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:05:29 +1100 Subject: [PATCH 215/293] mpu6000: update for new debug macros --- src/drivers/mpu6000/mpu6000.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6059096078df..a4310265a591 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -630,7 +630,7 @@ MPU6000::init() /* if probe/setup failed, bail now */ if (ret != OK) { - debug("SPI setup failed"); + DEVICE_DEBUG("SPI setup failed"); return ret; } @@ -666,7 +666,7 @@ MPU6000::init() ret = _gyro->init(); /* if probe/setup failed, bail now */ if (ret != OK) { - debug("gyro init failed"); + DEVICE_DEBUG("gyro init failed"); return ret; } @@ -783,7 +783,7 @@ MPU6000::probe() uint8_t expected = is_mpu_device() ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608; if (whoami != expected) { - debug("unexpected WHOAMI 0x%02x", whoami); + DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami); return -EIO; } @@ -805,12 +805,12 @@ MPU6000::probe() case MPU6000_REV_D9: case MPU6000_REV_D10: case ICM20608_REV_00: - debug("ID 0x%02x", _product); + DEVICE_DEBUG("ID 0x%02x", _product); _checked_values[0] = _product; return OK; } - debug("unexpected ID 0x%02x", _product); + DEVICE_DEBUG("unexpected ID 0x%02x", _product); return -EIO; } @@ -1930,7 +1930,7 @@ MPU6000_gyro::init() /* if probe/setup failed, bail now */ if (ret != OK) { - debug("gyro init failed"); + DEVICE_DEBUG("gyro init failed"); return ret; } From 9109024f92937657d1a0f892eafd9ad46addf925 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:05:39 +1100 Subject: [PATCH 216/293] mpu9250: update from upstream --- src/drivers/mpu9250/CMakeLists.txt | 45 +++ src/drivers/mpu9250/mpu9250.cpp | 487 +++++++++++++++++++---------- 2 files changed, 365 insertions(+), 167 deletions(-) create mode 100644 src/drivers/mpu9250/CMakeLists.txt diff --git a/src/drivers/mpu9250/CMakeLists.txt b/src/drivers/mpu9250/CMakeLists.txt new file mode 100644 index 000000000000..0c1b6e6fb832 --- /dev/null +++ b/src/drivers/mpu9250/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__mpu9250 + MAIN mpu9250 + STACK 1200 + COMPILE_FLAGS + -Weffc++ + -Os + SRCS + mpu9250.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 0a9937a36048..002b0dbf68c5 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -70,6 +70,7 @@ #include #include +#include #include #include #include @@ -173,12 +174,16 @@ #define MPU_WHOAMI_9250 0x71 -#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41 #define MPU9250_ACCEL_DEFAULT_RATE 1000 +#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 #define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 #define MPU9250_GYRO_DEFAULT_RATE 1000 +/* rates need to be the same between accel and gyro */ +#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE #define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 +#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41 + #define MPU9250_ONE_G 9.80665f #ifdef PX4_SPI_BUS_EXT @@ -280,6 +285,9 @@ class MPU9250 : public device::SPI math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + Integrator _accel_int; + Integrator _gyro_int; + enum Rotation _rotation; // this is used to support runtime checking of key @@ -337,7 +345,7 @@ class MPU9250 : public device::SPI * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg, uint32_t speed=MPU9250_LOW_BUS_SPEED); + uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); /** @@ -392,7 +400,7 @@ class MPU9250 : public device::SPI * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /** * Accel self test @@ -406,7 +414,7 @@ class MPU9250 : public device::SPI * * @return 0 on success, 1 on failure */ - int gyro_self_test(); + int gyro_self_test(); /* set low pass filter frequency @@ -424,8 +432,8 @@ class MPU9250 : public device::SPI void check_registers(void); /* do not allow to copy this class due to pointer data members */ - MPU9250(const MPU9250&); - MPU9250 operator=(const MPU9250&); + MPU9250(const MPU9250 &); + MPU9250 operator=(const MPU9250 &); #pragma pack(push, 1) /** @@ -455,12 +463,13 @@ const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPU MPUREG_PWR_MGMT_2, MPUREG_USER_CTRL, MPUREG_SMPLRT_DIV, - MPUREG_CONFIG, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_ACCEL_CONFIG2, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG }; + MPUREG_INT_PIN_CFG + }; @@ -490,8 +499,8 @@ class MPU9250_gyro : public device::CDev int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ - MPU9250_gyro(const MPU9250_gyro&); - MPU9250_gyro operator=(const MPU9250_gyro&); + MPU9250_gyro(const MPU9250_gyro &); + MPU9250_gyro operator=(const MPU9250_gyro &); }; /** driver 'main' command */ @@ -534,6 +543,8 @@ MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE), + _gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true), _rotation(rotation), _checked_next(0), _last_temperature(0), @@ -577,13 +588,17 @@ MPU9250::~MPU9250() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_gyro_reports != nullptr) + } + + if (_gyro_reports != nullptr) { delete _gyro_reports; + } - if (_accel_class_instance != -1) + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -606,21 +621,26 @@ MPU9250::init() /* if probe/setup failed, bail now */ if (ret != OK) { - debug("SPI setup failed"); + DEVICE_DEBUG("SPI setup failed"); return ret; } /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + + if (_accel_reports == nullptr) { goto out; + } _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); - if (_gyro_reports == nullptr) + + if (_gyro_reports == nullptr) { goto out; + } - if (reset() != OK) + if (reset() != OK) { goto out; + } /* Initialize offsets and scales */ _accel_scale.x_offset = 0; @@ -640,9 +660,10 @@ MPU9250::init() /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); + /* if probe/setup failed, bail now */ if (ret != OK) { - debug("gyro init failed"); + DEVICE_DEBUG("gyro init failed"); return ret; } @@ -722,16 +743,20 @@ int MPU9250::reset() usleep(1000); uint8_t retries = 10; + while (retries--) { bool all_ok = true; - for (uint8_t i=0; i200) div=200; - if(div<1) div=1; - write_checked_reg(MPUREG_SMPLRT_DIV, div-1); + + if (div > 200) { div = 200; } + + if (div < 1) { div = 1; } + + write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; } @@ -790,31 +818,40 @@ MPU9250::_set_dlpf_filter(uint16_t frequency_hz) if (frequency_hz == 0) { _dlpf_freq = 0; filter = BITS_DLPF_CFG_3600HZ; + } else if (frequency_hz <= 5) { _dlpf_freq = 5; filter = BITS_DLPF_CFG_5HZ; + } else if (frequency_hz <= 10) { _dlpf_freq = 10; filter = BITS_DLPF_CFG_10HZ; + } else if (frequency_hz <= 20) { _dlpf_freq = 20; filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 41) { _dlpf_freq = 41; filter = BITS_DLPF_CFG_41HZ; + } else if (frequency_hz <= 92) { _dlpf_freq = 92; filter = BITS_DLPF_CFG_92HZ; + } else if (frequency_hz <= 184) { _dlpf_freq = 184; filter = BITS_DLPF_CFG_184HZ; + } else if (frequency_hz <= 250) { _dlpf_freq = 250; filter = BITS_DLPF_CFG_250HZ; + } else { _dlpf_freq = 0; filter = BITS_DLPF_CFG_3600HZ; } + write_checked_reg(MPUREG_CONFIG, filter); } @@ -824,8 +861,9 @@ MPU9250::read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -834,17 +872,21 @@ MPU9250::read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) + if (_accel_reports->empty()) { return -EAGAIN; + } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_accel_reports->get(arp)) + if (!_accel_reports->get(arp)) { break; + } + transferred++; arp++; } @@ -867,24 +909,34 @@ MPU9250::self_test() int MPU9250::accel_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.z_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { return 1; + } return 0; } @@ -892,8 +944,9 @@ MPU9250::accel_self_test() int MPU9250::gyro_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* * Maximum deviation of 20 degrees, according to @@ -909,27 +962,35 @@ MPU9250::gyro_self_test() 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) + if (fabsf(_gyro_scale.x_offset) > max_offset) { return 1; + } /* evaluate gyro scale, complain if off by more than 30% */ - if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > max_offset) + if (fabsf(_gyro_scale.y_offset) > max_offset) { return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + } + + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > max_offset) + if (fabsf(_gyro_scale.z_offset) > max_offset) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + } + + 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)) { + (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; } @@ -960,8 +1021,9 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -970,17 +1032,21 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) + if (_gyro_reports->empty()) { return -EAGAIN; + } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_gyro_reports->get(grp)) + if (!_gyro_reports->get(grp)) { break; + } + transferred++; grp++; } @@ -1000,27 +1066,27 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); @@ -1029,12 +1095,13 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; + float sample_rate = 1.0e6f / ticks; _set_dlpf_filter(cutoff_freq_hz); _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); @@ -1051,17 +1118,18 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call_interval = ticks; - /* - set call interval faster then the sample time. We - then detect when we have duplicate samples and reject - them. This prevents aliasing due to a beat between the - stm32 clock and the mpu9250 clock - */ - _call.period = _call_interval - MPU9250_TIMER_REDUCTION; + /* + set call interval faster then the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu9250 clock + */ + _call.period = _call_interval - MPU9250_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -1069,25 +1137,29 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; 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 (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_accel_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -1109,14 +1181,15 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; - case ACCELIOCSSCALE: - { + case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; + } else { return -EINVAL; } @@ -1131,17 +1204,23 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) return set_accel_range(arg); case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/MPU9250_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / MPU9250_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); +#ifdef ACCELIOCSHWLOWPASS + case ACCELIOCSHWLOWPASS: _set_dlpf_filter(arg); return OK; +#endif + +#ifdef ACCELIOCGHWLOWPASS case ACCELIOCGHWLOWPASS: return _dlpf_freq; +#endif default: @@ -1155,26 +1234,29 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - /* these are shared with the accel side */ + /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); 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 (!_gyro_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_gyro_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -1212,18 +1294,25 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_scale = xx // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); case GYROIOCSELFTEST: return gyro_self_test(); +#ifdef GYROIOCSHWLOWPASS + case GYROIOCSHWLOWPASS: _set_dlpf_filter(arg); return OK; +#endif + +#ifdef GYROIOCGHWLOWPASS case GYROIOCGHWLOWPASS: return _dlpf_freq; +#endif default: /* give it to the superclass */ @@ -1236,8 +1325,8 @@ MPU9250::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; - // general register transfer at low clock speed - set_frequency(speed); + // general register transfer at low clock speed + set_frequency(speed); transfer(cmd, cmd, sizeof(cmd)); @@ -1249,8 +1338,8 @@ MPU9250::read_reg16(unsigned reg) { uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; - // general register transfer at low clock speed - set_frequency(MPU9250_LOW_BUS_SPEED); + // general register transfer at low clock speed + set_frequency(MPU9250_LOW_BUS_SPEED); transfer(cmd, cmd, sizeof(cmd)); @@ -1265,8 +1354,8 @@ MPU9250::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - // general register transfer at low clock speed - set_frequency(MPU9250_LOW_BUS_SPEED); + // general register transfer at low clock speed + set_frequency(MPU9250_LOW_BUS_SPEED); transfer(cmd, nullptr, sizeof(cmd)); } @@ -1286,7 +1375,8 @@ void MPU9250::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; i 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; @@ -1338,9 +1431,9 @@ MPU9250::start() /* start polling at the specified rate */ hrt_call_every(&_call, - 1000, - _call_interval-MPU9250_TIMER_REDUCTION, - (hrt_callout)&MPU9250::measure_trampoline, this); + 1000, + _call_interval - MPU9250_TIMER_REDUCTION, + (hrt_callout)&MPU9250::measure_trampoline, this); } void @@ -1371,7 +1464,8 @@ MPU9250::check_registers(void) the data registers. */ uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != + + if ((v = read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { _checked_bad[_checked_next] = v; @@ -1400,7 +1494,8 @@ MPU9250::check_registers(void) // register values but large offsets on the // accel axes _reset_wait = hrt_absolute_time() + 10000; - _checked_next = 0; + _checked_next = 0; + } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); // waiting 3ms between register writes seems @@ -1408,9 +1503,11 @@ MPU9250::check_registers(void) // recovering considerably _reset_wait = hrt_absolute_time() + 3000; } + _register_wait = 20; } - _checked_next = (_checked_next+1) % MPU9250_NUM_CHECKED_REGISTERS; + + _checked_next = (_checked_next + 1) % MPU9250_NUM_CHECKED_REGISTERS; } void @@ -1422,6 +1519,7 @@ MPU9250::measure() } struct MPUReport mpu_report; + struct Report { int16_t accel_x; int16_t accel_y; @@ -1440,13 +1538,14 @@ MPU9250::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - // sensor transfer at high clock speed - set_frequency(MPU9250_HIGH_BUS_SPEED); + // sensor transfer at high clock speed + set_frequency(MPU9250_HIGH_BUS_SPEED); - if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { return; + } - check_registers(); + check_registers(); /* see if this is duplicate accelerometer data. Note that we @@ -1456,13 +1555,14 @@ MPU9250::measure() sampled at 8kHz, so we would incorrectly think we have new data when we are in fact getting duplicate accelerometer data. */ - if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { + if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { // it isn't new data - wait for next timer perf_end(_sample_perf); perf_count(_duplicates); _got_duplicate = true; return; } + memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); _got_duplicate = false; @@ -1490,10 +1590,10 @@ MPU9250::measure() // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); - // note that we don't call reset() here as a reset() - // costs 20ms with interrupts disabled. That means if - // the mpu6k does go bad it would cause a FMU failure, - // regardless of whether another sensor is available, + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, return; } @@ -1540,7 +1640,7 @@ MPU9250::measure() // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1579,6 +1679,14 @@ MPU9250::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); + math::Vector<3> aval(x_in_new, y_in_new, z_in_new); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); + arb.x_integral = aval_integrated(0); + arb.y_integral = aval_integrated(1); + arb.z_integral = aval_integrated(2); + arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1606,6 +1714,14 @@ MPU9250::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); + math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + math::Vector<3> gval_integrated; + + bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); + grb.x_integral = gval_integrated(0); + grb.y_integral = gval_integrated(1); + grb.z_integral = gval_integrated(2); + grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; @@ -1616,10 +1732,15 @@ MPU9250::measure() _gyro_reports->force(&grb); /* notify anyone waiting for data */ - poll_notify(POLLIN); - _gyro->parent_poll_notify(); + if (accel_notify) { + poll_notify(POLLIN); + } - if (!(_pub_blocked)) { + if (gyro_notify) { + _gyro->parent_poll_notify(); + } + + if (accel_notify && !(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); perf_begin(_system_latency_perf); @@ -1627,7 +1748,7 @@ MPU9250::measure() orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } - if (!(_pub_blocked)) { + if (gyro_notify && !(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } @@ -1649,22 +1770,26 @@ MPU9250::print_info() perf_print_counter(_duplicates); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); - ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i=0; igyro_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->gyro_ioctl(filp, cmd, arg); } } @@ -1770,48 +1900,55 @@ void start(bool external_bus, enum Rotation rotation) { int fd; - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; if (*g_dev_ptr != nullptr) /* if already started, the still command succeeded */ + { errx(0, "already started"); + } /* create the driver */ - if (external_bus) { + if (external_bus) { #ifdef PX4_SPI_BUS_EXT *g_dev_ptr = new MPU9250(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); #else errx(0, "External SPI not available"); #endif + } else { *g_dev_ptr = new MPU9250(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); } - if (*g_dev_ptr == nullptr) + if (*g_dev_ptr == nullptr) { goto fail; + } - if (OK != (*g_dev_ptr)->init()) + if (OK != (*g_dev_ptr)->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } - close(fd); + close(fd); exit(0); fail: if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; } errx(1, "driver start failed"); @@ -1820,14 +1957,17 @@ start(bool external_bus, enum Rotation rotation) void stop(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + if (*g_dev_ptr != nullptr) { delete *g_dev_ptr; *g_dev_ptr = nullptr; + } else { /* warn, but not an error */ warnx("already stopped."); } + exit(0); } @@ -1839,8 +1979,8 @@ stop(bool external_bus) void test(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; accel_report a_report; gyro_report g_report; ssize_t sz; @@ -1855,12 +1995,14 @@ test(bool external_bus) /* get the driver */ int fd_gyro = open(path_gyro, O_RDONLY); - if (fd_gyro < 0) + if (fd_gyro < 0) { err(1, "%s open failed", path_gyro); + } /* reset to manual polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); + } /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); @@ -1914,19 +2056,22 @@ test(bool external_bus) void reset(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; int fd = open(path_accel, 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"); + } - close(fd); + close(fd); exit(0); } @@ -1937,9 +2082,11 @@ reset(bool external_bus) void info(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_info(); @@ -1953,9 +2100,11 @@ info(bool external_bus) void regdump(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_registers(); @@ -1969,9 +2118,11 @@ regdump(bool external_bus) void testerror(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } (*g_dev_ptr)->test_error(); @@ -2002,9 +2153,11 @@ mpu9250_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + default: mpu9250::usage(); exit(0); From bdffd7a023f160d538c3b77f44f2cf84e54bd167 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:05:51 +1100 Subject: [PATCH 217/293] ms5611: update for new debug macros --- src/drivers/ms5611/ms5611_nuttx.cpp | 4 ++-- src/drivers/ms5611/ms5611_spi.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index 2e98de198292..35b851a9126a 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -264,7 +264,7 @@ MS5611::init() ret = CDev::init(); if (ret != OK) { - debug("CDev init failed"); + DEVICE_DEBUG("CDev init failed"); goto out; } @@ -272,7 +272,7 @@ MS5611::init() _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { - debug("can't get memory for reports"); + DEVICE_DEBUG("can't get memory for reports"); ret = -ENOMEM; goto out; } diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 7ec7718157ec..884b5d464fa7 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -146,21 +146,21 @@ MS5611_SPI::init() ret = SPI::init(); if (ret != OK) { - debug("SPI init failed"); + DEVICE_DEBUG("SPI init failed"); goto out; } /* send reset command */ ret = _reset(); if (ret != OK) { - debug("reset failed"); + DEVICE_DEBUG("reset failed"); goto out; } /* read PROM */ ret = _read_prom(); if (ret != OK) { - debug("prom readout failed"); + DEVICE_DEBUG("prom readout failed"); goto out; } @@ -251,16 +251,16 @@ MS5611_SPI::_read_prom() _prom.c[i] = _reg16(cmd); if (_prom.c[i] != 0) all_zero = false; - //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); + //DEVICE_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } /* calculate CRC and return success/failure accordingly */ int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO; if (ret != OK) { - debug("crc failed"); + DEVICE_DEBUG("crc failed"); } if (all_zero) { - debug("prom all zero"); + DEVICE_DEBUG("prom all zero"); ret = -EIO; } return ret; From 42000cc9b8f35453f24ab3c7371211f8a36870ba Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:06:06 +1100 Subject: [PATCH 218/293] pca8574: update from upstream --- src/drivers/pca8574/CMakeLists.txt | 43 ++++++++++++++++++++++++++++++ src/drivers/pca8574/pca8574.cpp | 9 +++++++ 2 files changed, 52 insertions(+) create mode 100644 src/drivers/pca8574/CMakeLists.txt diff --git a/src/drivers/pca8574/CMakeLists.txt b/src/drivers/pca8574/CMakeLists.txt new file mode 100644 index 000000000000..4b00076be487 --- /dev/null +++ b/src/drivers/pca8574/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__pca8574 + MAIN pca8574 + COMPILE_FLAGS + -Os + SRCS + pca8574.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp index c697125a7110..14ccaab7b2fa 100644 --- a/src/drivers/pca8574/pca8574.cpp +++ b/src/drivers/pca8574/pca8574.cpp @@ -188,6 +188,7 @@ PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) if (_values_out) { _mode = IOX_MODE_ON; } + send_led_values(); } @@ -277,15 +278,18 @@ PCA8574::led() _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; } @@ -304,6 +308,7 @@ PCA8574::led() msg |= ((_blink_phase) ? _blinking : 0); _blink_phase = !_blink_phase; + } else { msg = _values_out; } @@ -513,6 +518,7 @@ pca8574_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); fflush(stdout); @@ -520,6 +526,7 @@ pca8574_main(int argc, char *argv[]) delete g_pca8574; g_pca8574 = nullptr; exit(0); + } else { warnx("stop failed."); exit(1); @@ -542,9 +549,11 @@ pca8574_main(int argc, char *argv[]) if (channel < 8) { ret = ioctl(fd, (IOX_SET_VALUE + channel), val); + } else { ret = -1; } + close(fd); exit(ret); } From 508c1279908b925dbc5be635e2672b5c87fa07b8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:06:15 +1100 Subject: [PATCH 219/293] pca9685: update from upstream --- src/drivers/pca9685/CMakeLists.txt | 41 ++++++++++++++++++ src/drivers/pca9685/pca9685.cpp | 69 +++++++++++++++++++++++------- 2 files changed, 94 insertions(+), 16 deletions(-) create mode 100644 src/drivers/pca9685/CMakeLists.txt diff --git a/src/drivers/pca9685/CMakeLists.txt b/src/drivers/pca9685/CMakeLists.txt new file mode 100644 index 000000000000..e5053ab8f6a9 --- /dev/null +++ b/src/drivers/pca9685/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__pca9685 + MAIN pca9685 + SRCS + pca9685.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index c26e1b8fed3f..051b4602f708 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -117,7 +117,7 @@ static const int ERROR = -1; class PCA9685 : public device::I2C { public: - PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR); + PCA9685(int bus = PCA9685_BUS, uint8_t address = ADDR); virtual ~PCA9685(); @@ -192,7 +192,7 @@ PCA9685::PCA9685(int bus, uint8_t address) : I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000), _mode(IOX_MODE_OFF), _running(false), - _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), + _i2cpwm_interval(SEC2TICK(1.0f / 60.0f)), _should_run(false), _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")), _actuator_controls_sub(-1), @@ -213,11 +213,13 @@ PCA9685::init() { int ret; ret = I2C::init(); + if (ret != OK) { return ret; } ret = reset(); + if (ret != OK) { return ret; } @@ -231,6 +233,7 @@ int PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = -EINVAL; + switch (cmd) { case IOX_SET_MODE: @@ -241,9 +244,11 @@ PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg) case IOX_MODE_OFF: warnx("shutting down"); break; + case IOX_MODE_ON: warnx("starting"); break; + case IOX_MODE_TEST_OUT: warnx("test starting"); break; @@ -280,6 +285,7 @@ PCA9685::info() if (is_running()) { warnx("Driver is running, mode: %u", _mode); + } else { warnx("Driver started but not running"); } @@ -304,8 +310,10 @@ PCA9685::i2cpwm() if (_mode == IOX_MODE_TEST_OUT) { setPin(0, PCA9685_PWMCENTER); _should_run = true; + } else if (_mode == IOX_MODE_OFF) { _should_run = false; + } else { if (!_mode_on_initialized) { /* Subscribe to actuator control 2 (payload group for gimbal) */ @@ -319,25 +327,29 @@ PCA9685::i2cpwm() /* Read the servo setpoints from the actuator control topics (gimbal) */ bool updated; orb_check(_actuator_controls_sub, &updated); + if (updated) { orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); + for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { /* Scale the controls to PWM, first multiply by pi to get rad, * the control[i] values are on the range -1 ... 1 */ uint16_t new_value = PCA9685_PWMCENTER + - (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE); - debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, - (double)_actuator_controls.control[i]); + (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE); + DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, + (double)_actuator_controls.control[i]); + if (new_value != _current_values[i] && - isfinite(new_value) && - new_value >= PCA9685_PWMMIN && - new_value <= PCA9685_PWMMAX) { + isfinite(new_value) && + new_value >= PCA9685_PWMMIN && + new_value <= PCA9685_PWMMAX) { /* This value was updated, send the command to adjust the PWM value */ setPin(i, new_value); _current_values[i] = new_value; } } } + _should_run = true; } @@ -368,7 +380,7 @@ PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off) if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + DEVICE_LOG("i2c::transfer returned %d", ret); } return ret; @@ -381,23 +393,29 @@ PCA9685::setPin(uint8_t num, uint16_t val, bool invert) if (val > 4095) { val = 4095; } + if (invert) { if (val == 0) { // Special value for signal fully on. return setPWM(num, 4096, 0); + } else if (val == 4095) { // Special value for signal fully off. return setPWM(num, 0, 4096); + } else { - return setPWM(num, 0, 4095-val); + return setPWM(num, 0, 4095 - val); } + } else { if (val == 4095) { // Special value for signal fully on. return setPWM(num, 4096, 0); + } else if (val == 0) { // Special value for signal fully off. return setPWM(num, 0, 4096); + } else { return setPWM(num, 0, val); } @@ -419,20 +437,27 @@ PCA9685::setPWMFreq(float freq) uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor() uint8_t oldmode; ret = read8(PCA9685_MODE1, oldmode); + if (ret != OK) { return ret; } - uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep + + uint8_t newmode = (oldmode & 0x7F) | 0x10; // sleep ret = write8(PCA9685_MODE1, newmode); // go to sleep + if (ret != OK) { return ret; } + ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler + if (ret != OK) { return ret; } + ret = write8(PCA9685_MODE1, oldmode); + if (ret != OK) { return ret; } @@ -440,6 +465,7 @@ PCA9685::setPWMFreq(float freq) usleep(5000); //5ms delay (from arduino driver) ret = write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment. + if (ret != OK) { return ret; } @@ -455,12 +481,14 @@ PCA9685::read8(uint8_t addr, uint8_t &value) /* send addr */ ret = transfer(&addr, sizeof(addr), nullptr, 0); + if (ret != OK) { goto fail_read; } /* get value */ ret = transfer(nullptr, 0, &value, 1); + if (ret != OK) { goto fail_read; } @@ -469,28 +497,32 @@ PCA9685::read8(uint8_t addr, uint8_t &value) fail_read: perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + DEVICE_LOG("i2c::transfer returned %d", ret); return ret; } -int PCA9685::reset(void) { +int PCA9685::reset(void) +{ warnx("resetting"); return write8(PCA9685_MODE1, 0x0); } /* Wrapper to wite a byte to addr */ int -PCA9685::write8(uint8_t addr, uint8_t value) { +PCA9685::write8(uint8_t addr, uint8_t value) +{ int ret = OK; _msg[0] = addr; _msg[1] = value; /* send addr and value */ ret = transfer(_msg, 2, nullptr, 0); + if (ret != OK) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + DEVICE_LOG("i2c::transfer returned %d", ret); } + return ret; } @@ -571,10 +603,13 @@ pca9685_main(int argc, char *argv[]) errx(1, "init failed"); } } + fd = open(PCA9685_DEVICE_PATH, 0); + if (fd == -1) { errx(1, "Unable to open " PCA9685_DEVICE_PATH); } + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); close(fd); @@ -632,14 +667,16 @@ pca9685_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); fflush(stdout); if (!g_pca9685->is_running()) { delete g_pca9685; - g_pca9685= nullptr; + g_pca9685 = nullptr; warnx("stopped, exiting"); exit(0); + } else { warnx("stop failed."); exit(1); From a236bcf680e7e37e063f8e49b9545168627c3bd0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:06:31 +1100 Subject: [PATCH 220/293] px4flow: update from upstream --- src/drivers/px4flow/CMakeLists.txt | 45 ++++++++ src/drivers/px4flow/i2c_frame.h | 55 +++++----- src/drivers/px4flow/px4flow.cpp | 169 +++++++++++++++++++---------- 3 files changed, 182 insertions(+), 87 deletions(-) create mode 100644 src/drivers/px4flow/CMakeLists.txt diff --git a/src/drivers/px4flow/CMakeLists.txt b/src/drivers/px4flow/CMakeLists.txt new file mode 100644 index 000000000000..21bfa4526e51 --- /dev/null +++ b/src/drivers/px4flow/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__px4flow + MAIN px4flow + STACK 1200 + COMPILE_FLAGS + -Wno-attributes + -Os + SRCS + px4flow.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h index b391b1f6a87f..f73d4a92cc57 100644 --- a/src/drivers/px4flow/i2c_frame.h +++ b/src/drivers/px4flow/i2c_frame.h @@ -40,41 +40,40 @@ #ifndef I2C_FRAME_H_ #define I2C_FRAME_H_ -#include +#define __STDC_FORMAT_MACROS +#include -typedef struct i2c_frame -{ - uint16_t frame_count; - int16_t pixel_flow_x_sum; - int16_t pixel_flow_y_sum; - int16_t flow_comp_m_x; - int16_t flow_comp_m_y; - int16_t qual; - int16_t gyro_x_rate; - int16_t gyro_y_rate; - int16_t gyro_z_rate; - uint8_t gyro_range; - uint8_t sonar_timestamp; - int16_t ground_distance; +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; +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)) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 85e0e1da775b..5b7f1e5557ad 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -216,6 +216,10 @@ PX4FLOW::~PX4FLOW() if (_reports != nullptr) { delete _reports; } + + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int @@ -225,34 +229,32 @@ PX4FLOW::init() /* do I2C init (and probe) first */ if (I2C::init() != OK) { - goto out; + return ret; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) { - goto out; + return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report = {}; - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_HIGH); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_HIGH); - if (_distance_sensor_topic == nullptr) { - log("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor 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; } @@ -452,7 +454,7 @@ PX4FLOW::measure() if (OK != ret) { perf_count(_comms_errors); - debug("i2c::transfer returned %d", ret); + DEVICE_DEBUG("i2c::transfer returned %d", ret); return ret; } @@ -480,7 +482,7 @@ PX4FLOW::collect() } if (ret < 0) { - debug("error reading from sensor: %d", ret); + DEVICE_DEBUG("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -499,22 +501,34 @@ PX4FLOW::collect() struct optical_flow_s report; report.timestamp = hrt_absolute_time(); + 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 = 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 = 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.sonar_timestamp;//microseconds + report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; /* rotate measurements according to parameter */ float zeroval = 0.0f; + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); if (_px4flow_topic == nullptr) { @@ -598,12 +612,12 @@ void PX4FLOW::cycle() { if (OK != measure()) { - debug("measure error"); + DEVICE_DEBUG("measure error"); } /* perform collection */ if (OK != collect()) { - debug("collection error"); + DEVICE_DEBUG("collection error"); /* restart the measurement state machine */ start(); return; @@ -636,9 +650,13 @@ namespace px4flow #endif const int ERROR = -1; -PX4FLOW *g_dev; +PX4FLOW *g_dev = nullptr; +bool start_in_progress = false; + +const int START_RETRY_COUNT = 5; +const int START_RETRY_TIMEOUT = 1000; -void start(); +int start(); void stop(); void test(); void reset(); @@ -647,75 +665,109 @@ void info(); /** * Start the driver. */ -void +int start() { int fd; - if (g_dev != nullptr) { - errx(1, "already started"); + /* entry check: */ + if (start_in_progress) { + warnx("start already in progress"); + return 1; } - /* create the driver */ - g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION); + start_in_progress = true; - if (g_dev == nullptr) { - goto fail; + if (g_dev != nullptr) { + start_in_progress = false; + warnx("already started"); + return 1; } - if (OK != g_dev->init()) { + warnx("scanning I2C buses for device.."); - #ifdef PX4_I2C_BUS_ESC - delete g_dev; - /* try 2nd bus */ - g_dev = new PX4FLOW(PX4_I2C_BUS_ESC); + int retry_nr = 0; - if (g_dev == nullptr) { - goto fail; - } + while (1) { + const int busses_to_try[] = { + PX4_I2C_BUS_EXPANSION, +#ifdef PX4_I2C_BUS_ESC + PX4_I2C_BUS_ESC, +#endif +#ifdef PX4_I2C_BUS_ONBOARD + PX4_I2C_BUS_ONBOARD, +#endif + -1 + }; - if (OK != g_dev->init()) { - #endif + const int *cur_bus = busses_to_try; - delete g_dev; -#ifdef PX4_I2C_BUS_ONBOARD - /* try 3rd bus */ - g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD); + while (*cur_bus != -1) { + /* create the driver */ + /* warnx("trying bus %d", *cur_bus); */ + g_dev = new PX4FLOW(*cur_bus); if (g_dev == nullptr) { - goto fail; + /* this is a fatal error */ + break; } - if (OK != g_dev->init()) { - goto fail; + /* init the driver: */ + if (OK == g_dev->init()) { + /* success! */ + break; } -#endif - #ifdef PX4_I2C_BUS_ESC + + /* destroy it again because it failed. */ + delete g_dev; + g_dev = nullptr; + + /* try next! */ + cur_bus++; } - #endif - } - /* set the poll rate to default, starts automatic data collection */ - fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); + /* check whether we found it: */ + if (*cur_bus != -1) { - if (fd < 0) { - goto fail; - } + /* check for failure: */ + if (g_dev == nullptr) { + break; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { - goto fail; - } + /* set the poll rate to default, starts automatic data collection */ + fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); - exit(0); + if (fd < 0) { + break; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { + break; + } -fail: + /* success! */ + start_in_progress = false; + return 0; + } + + if (retry_nr < START_RETRY_COUNT) { + /* lets not be too verbose */ + // warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr); + usleep(START_RETRY_TIMEOUT * 1000); + retry_nr++; + + } else { + break; + } + } if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } - errx(1, "no PX4FLOW connected over I2C"); + start_in_progress = false; + return 1; } /** @@ -757,8 +809,7 @@ test() /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) - { + if (sz != sizeof(report)) { warnx("immediate read failed"); } @@ -865,7 +916,7 @@ px4flow_main(int argc, char *argv[]) * Start/load the driver. */ if (!strcmp(argv[1], "start")) { - px4flow::start(); + return px4flow::start(); } /* From f8f0d9014d14fcfdfc2900d21666fb2ea96d20f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:06:42 +1100 Subject: [PATCH 221/293] rgbled: update from upstream --- src/drivers/rgbled/rgbled.cpp | 84 +++++++++++++++++++++++------- src/drivers/rgbled/rgbled_params.c | 54 +++++++++++++++++++ 2 files changed, 119 insertions(+), 19 deletions(-) create mode 100644 src/drivers/rgbled/rgbled_params.c diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 649125301794..3cf33e8ecad8 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -101,11 +101,13 @@ class RGBLED : public device::I2C uint8_t _g; uint8_t _b; float _brightness; + float _max_brightness; bool _running; int _led_interval; bool _should_run; int _counter; + int _param_sub; void set_color(rgbled_color_t ledcolor); void set_mode(rgbled_mode_t mode); @@ -117,6 +119,7 @@ class RGBLED : public device::I2C int send_led_enable(bool enable); int send_led_rgb(); int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); + void update_params(); }; /* for now, we only support one RGBLED */ @@ -132,18 +135,20 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled #ifdef __PX4_NUTTX - ,100000 /* maximum speed supported */ + , 100000 /* maximum speed supported */ #endif - ), + ), _mode(RGBLED_MODE_OFF), _r(0), _g(0), _b(0), _brightness(1.0f), + _max_brightness(1.0f), _running(false), _led_interval(0), _should_run(false), - _counter(0) + _counter(0), + _param_sub(-1) { memset(&_work, 0, sizeof(_work)); memset(&_pattern, 0, sizeof(_pattern)); @@ -190,9 +195,9 @@ RGBLED::probe() unsigned prevretries = _retries; _retries = 4; - if ((ret=get(on, powersave, r, g, b)) != OK || - (ret=send_led_enable(false) != OK) || - (ret=send_led_enable(false) != OK)) { + if ((ret = get(on, powersave, r, g, b)) != OK || + (ret = send_led_enable(false) != OK) || + (ret = send_led_enable(false) != OK)) { return ret; } @@ -212,8 +217,8 @@ RGBLED::info() if (ret == OK) { /* we don't care about power-save mode */ - log("state: %s", on ? "ON" : "OFF"); - log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); + DEVICE_LOG("state: %s", on ? "ON" : "OFF"); + DEVICE_LOG("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); } else { warnx("failed to read led"); @@ -285,12 +290,30 @@ RGBLED::led() return; } + if (_param_sub < 0) { + _param_sub = orb_subscribe(ORB_ID(parameter_update)); + } + + if (_param_sub >= 0) { + bool updated = false; + orb_check(_param_sub, &updated); + + if (updated) { + parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + update_params(); + // Immediately update to change brightness + send_led_rgb(); + } + } + switch (_mode) { case RGBLED_MODE_BLINK_SLOW: case RGBLED_MODE_BLINK_NORMAL: case RGBLED_MODE_BLINK_FAST: - if (_counter >= 2) + if (_counter >= 2) { _counter = 0; + } send_led_enable(_counter == 0); @@ -298,8 +321,9 @@ RGBLED::led() case RGBLED_MODE_BREATHE: - if (_counter >= 62) + if (_counter >= 62) { _counter = 0; + } int n; @@ -317,8 +341,9 @@ RGBLED::led() case RGBLED_MODE_PATTERN: /* don't run out of the pattern array and stop if the next frame is 0 */ - if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) + if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) { _counter = 0; + } set_color(_pattern.color[_counter]); send_led_rgb(); @@ -528,8 +553,9 @@ RGBLED::send_led_enable(bool enable) { uint8_t settings_byte = 0; - if (enable) + if (enable) { settings_byte |= SETTING_ENABLE; + } settings_byte |= SETTING_NOT_POWERSAVE; @@ -546,9 +572,9 @@ RGBLED::send_led_rgb() { /* To scale from 0..255 -> 0..15 shift right by 4 bits */ const uint8_t msg[6] = { - SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4), - SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4), - SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4) + SUB_ADDR_PWM0, static_cast((_b >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM1, static_cast((_g >> 4) * _brightness * _max_brightness + 0.5f), + SUB_ADDR_PWM2, static_cast((_r >> 4) * _brightness * _max_brightness + 0.5f) }; return transfer(msg, sizeof(msg), nullptr, 0); } @@ -573,6 +599,22 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) return ret; } +void +RGBLED::update_params() +{ + int32_t maxbrt = 15; + param_get(param_find("LED_RGB_MAXBRT"), &maxbrt); + maxbrt = maxbrt > 15 ? 15 : maxbrt; + maxbrt = maxbrt < 0 ? 0 : maxbrt; + + // A minimum of 2 "on" steps is required for breathe effect + if (maxbrt == 1) { + maxbrt = 2; + } + + _max_brightness = maxbrt / 15.0f; +} + void rgbled_usage() { @@ -593,6 +635,7 @@ rgbled_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'a': @@ -609,10 +652,10 @@ rgbled_main(int argc, char *argv[]) } } - if (myoptind >= argc) { - rgbled_usage(); - return 1; - } + if (myoptind >= argc) { + rgbled_usage(); + return 1; + } const char *verb = argv[myoptind]; @@ -641,6 +684,7 @@ rgbled_main(int argc, char *argv[]) warnx("no RGB led on bus #%d", i2cdevice); return 1; } + i2cdevice = PX4_I2C_BUS_LED; } } @@ -705,12 +749,14 @@ rgbled_main(int argc, char *argv[]) ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); px4_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; return 0; } + return ret; } diff --git a/src/drivers/rgbled/rgbled_params.c b/src/drivers/rgbled/rgbled_params.c new file mode 100644 index 000000000000..5ed2c9de385e --- /dev/null +++ b/src/drivers/rgbled/rgbled_params.c @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file rgbled_params.c + * + * Parameters defined by the RBG led driver + * + * @author Nate Weibley + */ + + +#include +#include + +/** + * RGB Led brightness limit + * + * Set to 0 to disable, 1 for minimum brightness up to 15 (max) + * + * @min 0 + * @max 15 + */ +PARAM_DEFINE_INT32(LED_RGB_MAXBRT, 15); From df31210998a6c9230a588db3d0c93f3c3411c915 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:06:54 +1100 Subject: [PATCH 222/293] roboclaw: update from upstream --- src/drivers/roboclaw/CMakeLists.txt | 44 +++++++++ src/drivers/roboclaw/RoboClaw.cpp | 121 +++++++++++++++++-------- src/drivers/roboclaw/RoboClaw.hpp | 16 ++-- src/drivers/roboclaw/roboclaw_main.cpp | 25 ++--- 4 files changed, 149 insertions(+), 57 deletions(-) create mode 100644 src/drivers/roboclaw/CMakeLists.txt diff --git a/src/drivers/roboclaw/CMakeLists.txt b/src/drivers/roboclaw/CMakeLists.txt new file mode 100644 index 000000000000..c2eac724e636 --- /dev/null +++ b/src/drivers/roboclaw/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__roboclaw + MAIN roboclaw + COMPILE_FLAGS + -Os + SRCS + roboclaw_main.cpp + RoboClaw.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 5b945e452d22..f9116f1d45ee 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -61,7 +61,7 @@ uint8_t RoboClaw::checksum_mask = 0x7f; RoboClaw::RoboClaw(const char *deviceName, uint16_t address, - uint16_t pulsesPerRev): + uint16_t pulsesPerRev): _address(address), _pulsesPerRev(pulsesPerRev), _uart(0), @@ -80,18 +80,27 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, // start serial port _uart = open(deviceName, O_RDWR | O_NOCTTY); - if (_uart < 0) err(1, "could not open %s", deviceName); + + if (_uart < 0) { err(1, "could not open %s", deviceName); } + int ret = 0; struct termios uart_config; ret = tcgetattr(_uart, &uart_config); - if (ret < 0) err (1, "failed to get attr"); + + if (ret < 0) { err(1, "failed to get attr"); } + uart_config.c_oflag &= ~ONLCR; // no CR for every LF ret = cfsetispeed(&uart_config, B38400); - if (ret < 0) err (1, "failed to set input speed"); + + if (ret < 0) { err(1, "failed to set input speed"); } + ret = cfsetospeed(&uart_config, B38400); - if (ret < 0) err (1, "failed to set output speed"); + + if (ret < 0) { err(1, "failed to set output speed"); } + ret = tcsetattr(_uart, TCSANOW, &uart_config); - if (ret < 0) err (1, "failed to set attr"); + + if (ret < 0) { err(1, "failed to set attr"); } // setup default settings, reset encoders resetEncoders(); @@ -107,25 +116,30 @@ RoboClaw::~RoboClaw() int RoboClaw::readEncoder(e_motor motor) { uint16_t sum = 0; + if (motor == MOTOR_1) { _sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum); + } else if (motor == MOTOR_2) { _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); } + uint8_t rbuf[50]; usleep(5000); int nread = read(_uart, rbuf, 50); + if (nread < 6) { printf("failed to read\n"); return -1; } + //printf("received: \n"); //for (int i=0;i 1) value = 1; - if (value < -1) value = -1; - uint8_t speed = fabs(value)*127; + if (value > 1) { value = 1; } + + if (value < -1) { value = -1; } + + uint8_t speed = fabs(value) * 127; + // send command if (motor == MOTOR_1) { if (value > 0) { return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + } else { return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); } + } else if (motor == MOTOR_2) { if (value > 0) { return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + } else { return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); } } + return -1; } int RoboClaw::setMotorDutyCycle(e_motor motor, float value) { uint16_t sum = 0; + // bound - if (value > 1) value = 1; - if (value < -1) value = -1; - int16_t duty = 1500*value; + if (value > 1) { value = 1; } + + if (value < -1) { value = -1; } + + int16_t duty = 1500 * value; + // send command if (motor == MOTOR_1) { return _sendCommand(CMD_SIGNED_DUTYCYCLE_1, - (uint8_t *)(&duty), 2, sum); + (uint8_t *)(&duty), 2, sum); + } else if (motor == MOTOR_2) { return _sendCommand(CMD_SIGNED_DUTYCYCLE_2, - (uint8_t *)(&duty), 2, sum); + (uint8_t *)(&duty), 2, sum); } + return -1; } @@ -247,7 +285,7 @@ int RoboClaw::resetEncoders() { uint16_t sum = 0; return _sendCommand(CMD_RESET_ENCODERS, - nullptr, 0, sum); + nullptr, 0, sum); } int RoboClaw::update() @@ -256,52 +294,57 @@ int RoboClaw::update() // check for exit condition every second // note "::poll" is required to distinguish global // poll from member function for driver - if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error + if (::poll(&_controlPoll, 1, 1000) < 0) { return -1; } // poll error // if new data, send to motors if (_actuators.updated()) { _actuators.update(); - setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); - setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + setMotorDutyCycle(MOTOR_1, _actuators.get().control[CH_VOLTAGE_LEFT]); + setMotorDutyCycle(MOTOR_2, _actuators.get().control[CH_VOLTAGE_RIGHT]); } + return 0; } -uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) +uint16_t RoboClaw::_sumBytes(uint8_t *buf, size_t n) { uint16_t sum = 0; + //printf("sum\n"); - for (size_t i=0;i Date: Thu, 26 Nov 2015 10:07:04 +1100 Subject: [PATCH 223/293] sf0x: update from upstream --- src/drivers/sf0x/CMakeLists.txt | 44 +++++++++++++++++++++++++++++++++ src/drivers/sf0x/sf0x.cpp | 40 +++++++++++++++++------------- 2 files changed, 67 insertions(+), 17 deletions(-) create mode 100644 src/drivers/sf0x/CMakeLists.txt diff --git a/src/drivers/sf0x/CMakeLists.txt b/src/drivers/sf0x/CMakeLists.txt new file mode 100644 index 000000000000..a90d1da8243b --- /dev/null +++ b/src/drivers/sf0x/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__sf0x + MAIN sf0x + COMPILE_FLAGS + -Os + SRCS + sf0x.cpp + sf0x_parser.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 5b1b37f56331..41730618b25f 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -227,6 +227,7 @@ SF0X::SF0X(const char *port) : /* clear ONLCR flag (which appends a CR for every LF) */ uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ uart_config.c_cflag &= ~(CSTOPB | PARENB); @@ -281,10 +282,12 @@ SF0X::init() /* do regular cdev init */ ret = CDev::init(); - if (ret != OK) break; + + if (ret != OK) { break; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); + if (_reports == nullptr) { warnx("mem err"); ret = -1; @@ -293,19 +296,17 @@ SF0X::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report = {}; - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_HIGH); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_HIGH); - if (_distance_sensor_topic == nullptr) { - log("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } - } while(0); + } while (0); /* close the fd */ ::close(_fd); @@ -532,7 +533,7 @@ SF0X::measure() if (ret != sizeof(cmd)) { perf_count(_comms_errors); - log("write fail %d", ret); + DEVICE_LOG("write fail %d", ret); return ret; } @@ -559,7 +560,7 @@ SF0X::collect() ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { - debug("read err: %d", ret); + DEVICE_DEBUG("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); @@ -570,6 +571,7 @@ SF0X::collect() } else { return -EAGAIN; } + } else if (ret == 0) { return -EAGAIN; } @@ -589,7 +591,7 @@ SF0X::collect() return -EAGAIN; } - debug("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO")); + DEVICE_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO")); struct distance_sensor_s report; @@ -689,13 +691,15 @@ SF0X::cycle() /* we know the sensor needs about four seconds to initialize */ if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { - log("collection error #%u", _consecutive_fail_count); + DEVICE_LOG("collection error #%u", _consecutive_fail_count); } + _consecutive_fail_count++; /* restart the measurement state machine */ start(); return; + } else { /* apparently success */ _consecutive_fail_count = 0; @@ -722,7 +726,7 @@ SF0X::cycle() /* measurement phase */ if (OK != measure()) { - log("measure error"); + DEVICE_LOG("measure error"); } /* next phase is collection */ @@ -854,7 +858,7 @@ test() } warnx("single read"); - warnx("val: %0.2f m", (double)report.current_distance); + warnx("measurement: %0.2f m", (double)report.current_distance); warnx("time: %llu", report.timestamp); /* start the sensor polling at 2 Hz rate */ @@ -885,7 +889,9 @@ test() } warnx("read #%u", i); - warnx("val: %0.3f m", (double)report.current_distance); + warnx("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %llu", report.timestamp); } From bd5b7c931d84bc7701e1d110135418bf5287ba4f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:07:19 +1100 Subject: [PATCH 224/293] stm32: update from upstream --- src/drivers/stm32/adc/CMakeLists.txt | 43 +++ src/drivers/stm32/adc/adc.cpp | 6 +- src/drivers/stm32/drv_hrt.c | 56 ++-- src/drivers/stm32/drv_pwm_servo.c | 38 ++- src/drivers/stm32/tone_alarm/CMakeLists.txt | 43 +++ src/drivers/stm32/tone_alarm/tone_alarm.cpp | 334 ++++++++++++++------ 6 files changed, 396 insertions(+), 124 deletions(-) create mode 100644 src/drivers/stm32/adc/CMakeLists.txt create mode 100644 src/drivers/stm32/tone_alarm/CMakeLists.txt diff --git a/src/drivers/stm32/adc/CMakeLists.txt b/src/drivers/stm32/adc/CMakeLists.txt new file mode 100644 index 000000000000..72205cdac744 --- /dev/null +++ b/src/drivers/stm32/adc/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__stm32__adc + MAIN adc + COMPILE_FLAGS + -Os + SRCS + adc.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 164b36ab8980..9c6e60947a53 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -240,13 +240,13 @@ ADC::init() /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ if ((hrt_absolute_time() - now) > 500) { - log("sample timeout"); + DEVICE_LOG("sample timeout"); return -1; } } - debug("init done"); + DEVICE_DEBUG("init done"); /* create the device node */ return CDev::init(); @@ -405,7 +405,7 @@ ADC::_sample(unsigned channel) /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ if ((hrt_absolute_time() - now) > 50) { - log("sample timeout"); + DEVICE_LOG("sample timeout"); return 0xffff; } } diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 17a69b03377e..475c9c9c758b 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -85,7 +85,7 @@ #elif HRT_TIMER == 2 # define HRT_TIMER_BASE STM32_TIM2_BASE # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN +# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM2EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM2 # define HRT_TIMER_CLOCK STM32_APB1_TIM2_CLKIN # if CONFIG_STM32_TIM2 @@ -103,7 +103,7 @@ #elif HRT_TIMER == 4 # define HRT_TIMER_BASE STM32_TIM4_BASE # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM4EN +# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM4EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM4 # define HRT_TIMER_CLOCK STM32_APB1_TIM4_CLKIN # if CONFIG_STM32_TIM4 @@ -112,7 +112,7 @@ #elif HRT_TIMER == 5 # define HRT_TIMER_BASE STM32_TIM5_BASE # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN +# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM5EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM5 # define HRT_TIMER_CLOCK STM32_APB1_TIM5_CLKIN # if CONFIG_STM32_TIM5 @@ -129,16 +129,16 @@ # endif #elif HRT_TIMER == 9 # define HRT_TIMER_BASE STM32_TIM9_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1BRK -# define HRT_TIMER_CLOCK STM32_APB1_TIM9_CLKIN +# define HRT_TIMER_CLOCK STM32_APB2_TIM9_CLKIN # if CONFIG_STM32_TIM9 # error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9 # endif #elif HRT_TIMER == 10 # define HRT_TIMER_BASE STM32_TIM10_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP # define HRT_TIMER_CLOCK STM32_APB2_TIM10_CLKIN @@ -147,7 +147,7 @@ # endif #elif HRT_TIMER == 11 # define HRT_TIMER_BASE STM32_TIM11_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR +# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM # define HRT_TIMER_CLOCK STM32_APB2_TIM11_CLKIN @@ -456,8 +456,9 @@ hrt_ppm_decode(uint32_t status) unsigned i; /* if we missed an edge, we have to give up */ - if (status & SR_OVF_PPM) + if (status & SR_OVF_PPM) { goto error; + } /* how long since the last edge? - this handles counter wrapping implicitely. */ width = count - ppm.last_edge; @@ -465,8 +466,10 @@ hrt_ppm_decode(uint32_t status) #if PPM_DEBUG ppm_edge_history[ppm_edge_next++] = width; - if (ppm_edge_next >= 32) + if (ppm_edge_next >= 32) { ppm_edge_next = 0; + } + #endif /* @@ -502,8 +505,9 @@ hrt_ppm_decode(uint32_t status) } else { /* frame channel count matches expected, let's use it */ if (ppm.next_channel > PPM_MIN_CHANNELS) { - for (i = 0; i < ppm.next_channel; i++) + for (i = 0; i < ppm.next_channel; i++) { ppm_buffer[i] = ppm_temp_buffer[i]; + } ppm_last_valid_decode = hrt_absolute_time(); @@ -528,8 +532,9 @@ hrt_ppm_decode(uint32_t status) case ARM: /* we expect a pulse giving us the first mark */ - if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too short or too long */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too short or too long */ + } /* record the mark timing, expect an inactive edge */ ppm.last_mark = ppm.last_edge; @@ -543,8 +548,9 @@ hrt_ppm_decode(uint32_t status) case INACTIVE: /* we expect a short pulse */ - if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too short or too long */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too short or too long */ + } /* this edge is not interesting, but now we are ready for the next mark */ ppm.phase = ACTIVE; @@ -558,17 +564,21 @@ hrt_ppm_decode(uint32_t status) #if PPM_DEBUG ppm_pulse_history[ppm_pulse_next++] = interval; - if (ppm_pulse_next >= 32) + if (ppm_pulse_next >= 32) { ppm_pulse_next = 0; + } + #endif /* if the mark-mark timing is out of bounds, abandon the frame */ - if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) { goto error; + } /* if we have room to store the value, do so */ - if (ppm.next_channel < PPM_MAX_CHANNELS) + if (ppm.next_channel < PPM_MAX_CHANNELS) { ppm_temp_buffer[ppm.next_channel++] = interval; + } ppm.phase = INACTIVE; break; @@ -669,8 +679,9 @@ hrt_absolute_time(void) * This simple test is sufficient due to the guarantee that * we are always called at least once per counter period. */ - if (count < last_count) + if (count < last_count) { base_time += HRT_COUNTER_PERIOD; + } /* save the count for next time */ last_count = count; @@ -801,8 +812,9 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte queue for the uninitialised entry->link but we don't do anything actually unsafe. */ - if (entry->deadline != 0) + if (entry->deadline != 0) { sq_rem(&entry->link, &callout_queue); + } entry->deadline = deadline; entry->period = interval; @@ -884,11 +896,13 @@ hrt_call_invoke(void) call = (struct hrt_call *)sq_peek(&callout_queue); - if (call == NULL) + if (call == NULL) { break; + } - if (call->deadline > now) + if (call->deadline > now) { break; + } sq_rem(&call->link, &callout_queue); //lldbg("call pop\n"); diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index 77f538774989..3a24f1992276 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -174,19 +174,22 @@ pwm_channel_init(unsigned channel) int up_pwm_servo_set(unsigned channel, servo_position_t value) { - if (channel >= PWM_SERVO_MAX_CHANNELS) + if (channel >= PWM_SERVO_MAX_CHANNELS) { return -1; + } unsigned timer = pwm_channels[channel].timer_index; /* test timer for validity */ if ((pwm_timers[timer].base == 0) || - (pwm_channels[channel].gpio == 0)) + (pwm_channels[channel].gpio == 0)) { return -1; + } /* configure the channel */ - if (value > 0) + if (value > 0) { value--; + } switch (pwm_channels[channel].timer_channel) { case 1: @@ -215,16 +218,18 @@ up_pwm_servo_set(unsigned channel, servo_position_t value) servo_position_t up_pwm_servo_get(unsigned channel) { - if (channel >= PWM_SERVO_MAX_CHANNELS) + if (channel >= PWM_SERVO_MAX_CHANNELS) { return 0; + } unsigned timer = pwm_channels[channel].timer_index; servo_position_t value = 0; /* test timer for validity */ if ((pwm_timers[timer].base == 0) || - (pwm_channels[channel].timer_channel == 0)) + (pwm_channels[channel].timer_channel == 0)) { return 0; + } /* configure the channel */ switch (pwm_channels[channel].timer_channel) { @@ -253,15 +258,17 @@ up_pwm_servo_init(uint32_t channel_mask) { /* do basic timer initialisation first */ for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { - if (pwm_timers[i].base != 0) + if (pwm_timers[i].base != 0) { pwm_timer_init(i); + } } /* now init channels */ for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { /* don't do init for disabled channels; this leaves the pin configs alone */ - if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0)) + if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0)) { pwm_channel_init(i); + } } return OK; @@ -278,13 +285,17 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) { /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ - if (rate < 1) + if (rate < 1) { return -ERANGE; - if (rate > 10000) + } + + if (rate > 10000) { return -ERANGE; + } - if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0)) + if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0)) { return ERROR; + } pwm_timer_set_rate(group, rate); @@ -294,8 +305,9 @@ up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) int up_pwm_servo_set_rate(unsigned rate) { - for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { up_pwm_servo_set_rate_group_update(i, rate); + } return 0; } @@ -306,9 +318,11 @@ up_pwm_servo_get_rate_group(unsigned group) unsigned channels = 0; for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { - if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group)) + if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group)) { channels |= (1 << i); + } } + return channels; } diff --git a/src/drivers/stm32/tone_alarm/CMakeLists.txt b/src/drivers/stm32/tone_alarm/CMakeLists.txt new file mode 100644 index 000000000000..d01a1ff59b50 --- /dev/null +++ b/src/drivers/stm32/tone_alarm/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__stm32__tone_alarm + MAIN tone_alarm + COMPILE_FLAGS + -Os + SRCS + tone_alarm.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index ca4794bb16b9..1a54e11809d0 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -47,16 +47,16 @@ * From Wikibooks: * * PLAY "[string expression]" - * + * * Used to play notes and a score ... The tones are indicated by letters A through G. - * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) * immediately after the note letter. See this example: - * + * * PLAY "C C# C C#" * * Whitespaces are ignored inside the string expression. There are also codes that - * set the duration, octave and tempo. They are all case-insensitive. PLAY executes - * the commands or notes the order in which they appear in the string. Any indicators + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators * that change the properties are effective for the notes following that indicator. * * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration @@ -66,15 +66,15 @@ * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. - * Remember that C- is equivalent to B. + * Remember that C- is equivalent to B. * < > Changes the current octave respectively down or up one level. * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. - * Pn Causes a silence (pause) for the length of note indicated (same as Ln). - * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. * It can be used for a pause as well. @@ -116,11 +116,28 @@ #include #include +#include + +/* Check that tone alarm and HRT timers are different */ +#if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER) +# if TONE_ALARM_TIMER == HRT_TIMER +# error TONE_ALARM_TIMER and HRT_TIMER must use different timers. +# endif +#endif /* Tone alarm configuration */ -#if TONE_ALARM_TIMER == 2 +#if TONE_ALARM_TIMER == 1 +# define TONE_ALARM_BASE STM32_TIM1_BASE +# define TONE_ALARM_CLOCK STM32_APB2_TIM1_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM1EN +# ifdef CONFIG_STM32_TIM1 +# error Must not set CONFIG_STM32_TIM1 when TONE_ALARM_TIMER is 1 +# endif +#elif TONE_ALARM_TIMER == 2 # define TONE_ALARM_BASE STM32_TIM2_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM2_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM2EN # ifdef CONFIG_STM32_TIM2 # error Must not set CONFIG_STM32_TIM2 when TONE_ALARM_TIMER is 2 @@ -128,6 +145,7 @@ #elif TONE_ALARM_TIMER == 3 # define TONE_ALARM_BASE STM32_TIM3_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM3_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM3EN # ifdef CONFIG_STM32_TIM3 # error Must not set CONFIG_STM32_TIM3 when TONE_ALARM_TIMER is 3 @@ -135,6 +153,7 @@ #elif TONE_ALARM_TIMER == 4 # define TONE_ALARM_BASE STM32_TIM4_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM4_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM4EN # ifdef CONFIG_STM32_TIM4 # error Must not set CONFIG_STM32_TIM4 when TONE_ALARM_TIMER is 4 @@ -142,33 +161,45 @@ #elif TONE_ALARM_TIMER == 5 # define TONE_ALARM_BASE STM32_TIM5_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM5_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM5EN # ifdef CONFIG_STM32_TIM5 # error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5 # endif +#elif TONE_ALARM_TIMER == 8 +# define TONE_ALARM_BASE STM32_TIM8_BASE +# define TONE_ALARM_CLOCK STM32_APB2_TIM8_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM8EN +# ifdef CONFIG_STM32_TIM8 +# error Must not set CONFIG_STM32_TIM8 when TONE_ALARM_TIMER is 8 +# endif #elif TONE_ALARM_TIMER == 9 # define TONE_ALARM_BASE STM32_TIM9_BASE -# define TONE_ALARM_CLOCK STM32_APB1_TIM9_CLKIN -# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM9EN +# define TONE_ALARM_CLOCK STM32_APB2_TIM9_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM9EN # ifdef CONFIG_STM32_TIM9 # error Must not set CONFIG_STM32_TIM9 when TONE_ALARM_TIMER is 9 # endif #elif TONE_ALARM_TIMER == 10 # define TONE_ALARM_BASE STM32_TIM10_BASE -# define TONE_ALARM_CLOCK STM32_APB1_TIM10_CLKIN -# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM10EN +# define TONE_ALARM_CLOCK STM32_APB2_TIM10_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM10EN # ifdef CONFIG_STM32_TIM10 # error Must not set CONFIG_STM32_TIM10 when TONE_ALARM_TIMER is 10 # endif #elif TONE_ALARM_TIMER == 11 # define TONE_ALARM_BASE STM32_TIM11_BASE -# define TONE_ALARM_CLOCK STM32_APB1_TIM11_CLKIN -# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM11EN +# define TONE_ALARM_CLOCK STM32_APB2_TIM11_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM11EN # ifdef CONFIG_STM32_TIM11 # error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11 # endif #else -# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver. +# error Must set TONE_ALARM_TIMER to one of the timers between 1 and 11 (inclusive) to use this driver. #endif #if TONE_ALARM_CHANNEL == 1 @@ -201,24 +232,49 @@ */ #define REG(_reg) (*(volatile uint32_t *)(TONE_ALARM_BASE + _reg)) -#define rCR1 REG(STM32_GTIM_CR1_OFFSET) -#define rCR2 REG(STM32_GTIM_CR2_OFFSET) -#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) -#define rDIER REG(STM32_GTIM_DIER_OFFSET) -#define rSR REG(STM32_GTIM_SR_OFFSET) -#define rEGR REG(STM32_GTIM_EGR_OFFSET) -#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) -#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) -#define rCCER REG(STM32_GTIM_CCER_OFFSET) -#define rCNT REG(STM32_GTIM_CNT_OFFSET) -#define rPSC REG(STM32_GTIM_PSC_OFFSET) -#define rARR REG(STM32_GTIM_ARR_OFFSET) -#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) -#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) -#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) -#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) -#define rDCR REG(STM32_GTIM_DCR_OFFSET) -#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) +#if TONE_ALARM_TIMER == 1 || TONE_ALARM_TIMER == 8 // Note: If using TIM1 or TIM8, then you are using the ADVANCED timers and NOT the GENERAL TIMERS, therefore different registers +# define rCR1 REG(STM32_ATIM_CR1_OFFSET) +# define rCR2 REG(STM32_ATIM_CR2_OFFSET) +# define rSMCR REG(STM32_ATIM_SMCR_OFFSET) +# define rDIER REG(STM32_ATIM_DIER_OFFSET) +# define rSR REG(STM32_ATIM_SR_OFFSET) +# define rEGR REG(STM32_ATIM_EGR_OFFSET) +# define rCCMR1 REG(STM32_ATIM_CCMR1_OFFSET) +# define rCCMR2 REG(STM32_ATIM_CCMR2_OFFSET) +# define rCCER REG(STM32_ATIM_CCER_OFFSET) +# define rCNT REG(STM32_ATIM_CNT_OFFSET) +# define rPSC REG(STM32_ATIM_PSC_OFFSET) +# define rARR REG(STM32_ATIM_ARR_OFFSET) +# define rRCR REG(STM32_ATIM_RCR_OFFSET) +# define rCCR1 REG(STM32_ATIM_CCR1_OFFSET) +# define rCCR2 REG(STM32_ATIM_CCR2_OFFSET) +# define rCCR3 REG(STM32_ATIM_CCR3_OFFSET) +# define rCCR4 REG(STM32_ATIM_CCR4_OFFSET) +# define rBDTR REG(STM32_ATIM_BDTR_OFFSET) +# define rDCR REG(STM32_ATIM_DCR_OFFSET) +# define rDMAR REG(STM32_ATIM_DMAR_OFFSET) +#else +# define rCR1 REG(STM32_GTIM_CR1_OFFSET) +# define rCR2 REG(STM32_GTIM_CR2_OFFSET) +# define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +# define rDIER REG(STM32_GTIM_DIER_OFFSET) +# define rSR REG(STM32_GTIM_SR_OFFSET) +# define rEGR REG(STM32_GTIM_EGR_OFFSET) +# define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +# define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +# define rCCER REG(STM32_GTIM_CCER_OFFSET) +# define rCNT REG(STM32_GTIM_CNT_OFFSET) +# define rPSC REG(STM32_GTIM_PSC_OFFSET) +# define rARR REG(STM32_GTIM_ARR_OFFSET) +# define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +# define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +# define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +# define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +# define rDCR REG(STM32_GTIM_DCR_OFFSET) +# define rDMAR REG(STM32_GTIM_DMAR_OFFSET) +#endif + +#define CBRK_BUZZER_KEY 782097 class ToneAlarm : public device::CDev { @@ -230,14 +286,21 @@ class ToneAlarm : public device::CDev virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); - inline const char *name(int tune) { + inline const char *name(int tune) + { return _tune_names[tune]; } + enum { + CBRK_OFF = 0, + CBRK_ON, + CBRK_UNINIT + }; + private: static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes - const char * _default_tunes[TONE_NUMBER_OF_TUNES]; - const char * _tune_names[TONE_NUMBER_OF_TUNES]; + const char *_default_tunes[TONE_NUMBER_OF_TUNES]; + const char *_tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -253,6 +316,7 @@ class ToneAlarm : public device::CDev unsigned _octave; unsigned _silence_length; // if nonzero, silence before next note bool _repeat; // if true, tune restarts at end + int _cbrk; //if true, no audio output hrt_call _note_call; // HRT callout for note completion @@ -261,8 +325,8 @@ class ToneAlarm : public device::CDev // unsigned note_to_divisor(unsigned note); - // Calculate the duration in microseconds of play and silence for a - // note given the current tempo, length and mode and the number of + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of // dots following in the play string. // unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); @@ -321,7 +385,8 @@ ToneAlarm::ToneAlarm() : _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), - _next(nullptr) + _next(nullptr), + _cbrk(CBRK_UNINIT) { // enable debug() calls //_debug_enabled = true; @@ -339,6 +404,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning _default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning _default_tunes[TONE_SINGLE_BEEP_TUNE] = "MFT100a8"; // single beep + _default_tunes[TONE_HOME_SET] = "MFT100L4>G#6A#6B#4"; _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone @@ -354,6 +420,7 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning _tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning _tune_names[TONE_SINGLE_BEEP_TUNE] = "beep"; // single beep + _tune_names[TONE_HOME_SET] = "home_set"; } ToneAlarm::~ToneAlarm() @@ -367,14 +434,15 @@ ToneAlarm::init() ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } /* configure the GPIO to the idle state */ stm32_configgpio(GPIO_TONE_ALARM_IDLE); /* clock/power on our timer */ - modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); + modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 0, TONE_ALARM_CLOCK_ENABLE); /* initialise the timer */ rCR1 = 0; @@ -387,6 +455,10 @@ ToneAlarm::init() rCCER = TONE_CCER; rDCR = 0; +#ifdef rBDTR // If using an advanced timer, you need to activate the output + rBDTR = ATIM_BDTR_MOE; // enable the main output of the advanced timer +#endif + /* toggle the CC output each time the count passes 1 */ TONE_rCCR = 1; @@ -396,7 +468,7 @@ ToneAlarm::init() /* make sure the timer is running */ rCR1 = GTIM_CR1_CEN; - debug("ready"); + DEVICE_DEBUG("ready"); return OK; } @@ -419,25 +491,31 @@ ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (note_length == 0) + if (note_length == 0) { note_length = 1; + } + unsigned note_period = whole_note_period / note_length; switch (_note_mode) { case MODE_NORMAL: silence = note_period / 8; break; + case MODE_STACCATO: silence = note_period / 4; break; + default: case MODE_LEGATO: silence = 0; break; } + note_period -= silence; unsigned dot_extension = note_period / 2; + while (dots--) { note_period += dot_extension; dot_extension /= 2; @@ -451,12 +529,14 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (rest_length == 0) + if (rest_length == 0) { rest_length = 1; + } unsigned rest_period = whole_note_period / rest_length; unsigned dot_extension = rest_period / 2; + while (dots--) { rest_period += dot_extension; dot_extension /= 2; @@ -468,6 +548,13 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) void ToneAlarm::start_note(unsigned note) { + // check if circuit breaker is enabled + if (_cbrk == CBRK_UNINIT) { + _cbrk = circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY); + } + + if (_cbrk != CBRK_OFF) { return; } + // compute the divisor unsigned divisor = note_to_divisor(note); @@ -546,115 +633,155 @@ ToneAlarm::next_note() while (note == 0) { // we always need at least one character from the string int c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_end; + } + _next++; switch (c) { case 'L': // select note length _note_length = next_number(); - if (_note_length < 1) + + if (_note_length < 1) { goto tune_error; + } + break; case 'O': // select octave _octave = next_number(); - if (_octave > 6) + + if (_octave > 6) { _octave = 6; + } + break; case '<': // decrease octave - if (_octave > 0) + if (_octave > 0) { _octave--; + } + break; case '>': // increase octave - if (_octave < 6) + if (_octave < 6) { _octave++; + } + break; case 'M': // select inter-note gap c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_error; + } + _next++; + switch (c) { case 'N': _note_mode = MODE_NORMAL; break; + case 'L': _note_mode = MODE_LEGATO; break; + case 'S': _note_mode = MODE_STACCATO; break; + case 'F': _repeat = false; break; + case 'B': _repeat = true; break; + default: goto tune_error; } + break; case 'P': // pause for a note length stop_note(); - hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(next_number(), next_dots()), - (hrt_callout)next_trampoline, - this); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); return; case 'T': { // change tempo - unsigned nt = next_number(); + unsigned nt = next_number(); - if ((nt >= 32) && (nt <= 255)) { - _tempo = nt; - } else { - goto tune_error; + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + + } else { + goto tune_error; + } + + break; } - break; - } case 'N': // play an arbitrary note note = next_number(); - if (note > 84) + + if (note > 84) { goto tune_error; + } + if (note == 0) { // this is a rest - pause for the current note length hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(_note_length, next_dots()), - (hrt_callout)next_trampoline, - this); - return; + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; } + break; case 'A'...'G': // play a note in the current octave note = _note_tab[c - 'A'] + (_octave * 12) + 1; c = next_char(); + switch (c) { case '#': // up a semitone case '+': - if (note < 84) + if (note < 84) { note++; + } + _next++; break; + case '-': // down a semitone - if (note > 1) + if (note > 1) { note--; + } + _next++; break; + default: // 0 / no next char here is OK break; } + // shorthand length notation note_length = next_number(); - if (note_length == 0) + + if (note_length == 0) { note_length = _note_length; + } + break; default: @@ -680,12 +807,15 @@ ToneAlarm::next_note() // stop (and potentially restart) the tune tune_end: stop_note(); + if (_repeat) { start_tune(_tune); + } else { _tune = nullptr; _default_tune_number = 0; } + return; } @@ -695,6 +825,7 @@ ToneAlarm::next_char() while (isspace(*_next)) { _next++; } + return toupper(*_next); } @@ -706,8 +837,11 @@ ToneAlarm::next_number() for (;;) { c = next_char(); - if (!isdigit(c)) + + if (!isdigit(c)) { return number; + } + _next++; number = (number * 10) + (c - '0'); } @@ -722,6 +856,7 @@ ToneAlarm::next_dots() _next++; dots++; } + return dots; } @@ -739,14 +874,14 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) { int result = OK; - debug("ioctl %i %u", cmd, arg); + DEVICE_DEBUG("ioctl %i %u", cmd, arg); // irqstate_t flags = irqsave(); /* decide whether to increase the alarm level to cmd or leave it alone */ switch (cmd) { case TONE_SET_ALARM: - debug("TONE_SET_ALARM %u", arg); + DEVICE_DEBUG("TONE_SET_ALARM %u", arg); if (arg < TONE_NUMBER_OF_TUNES) { if (arg == TONE_STOP_TUNE) { @@ -755,6 +890,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) _next = nullptr; _repeat = false; _default_tune_number = 0; + } else { /* always interrupt alarms, unless they are repeating and already playing */ if (!(_repeat && _default_tune_number == arg)) { @@ -763,6 +899,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) start_tune(_default_tunes[arg]); } } + } else { result = -EINVAL; } @@ -777,8 +914,9 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) // irqrestore(flags); /* give it to the superclass if we didn't like it */ - if (result == -ENOTTY) + if (result == -ENOTTY) { result = CDev::ioctl(filp, cmd, arg); + } return result; } @@ -787,8 +925,9 @@ int ToneAlarm::write(file *filp, const char *buffer, size_t len) { // sanity-check the buffer for length and nul-termination - if (len > _tune_max) + if (len > _tune_max) { return -EFBIG; + } // if we have an existing user tune, free it if (_user_tune != nullptr) { @@ -805,13 +944,16 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len) } // if the new tune is empty, we're done - if (buffer[0] == '\0') + if (buffer[0] == '\0') { return OK; + } // allocate a copy of the new tune _user_tune = strndup(buffer, len); - if (_user_tune == nullptr) + + if (_user_tune == nullptr) { return -ENOMEM; + } // and play it start_tune(_user_tune); @@ -834,14 +976,16 @@ play_tune(unsigned tune) fd = open(TONEALARM0_DEVICE_PATH, 0); - if (fd < 0) + if (fd < 0) { err(1, TONEALARM0_DEVICE_PATH); + } ret = ioctl(fd, TONE_SET_ALARM, tune); close(fd); - if (ret != 0) + if (ret != 0) { err(1, "TONE_SET_ALARM"); + } exit(0); } @@ -853,17 +997,21 @@ play_string(const char *str, bool free_buffer) fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY); - if (fd < 0) + if (fd < 0) { err(1, TONEALARM0_DEVICE_PATH); + } ret = write(fd, str, strlen(str) + 1); close(fd); - if (free_buffer) + if (free_buffer) { free((void *)str); + } - if (ret < 0) + if (ret < 0) { err(1, "play tune"); + } + exit(0); } @@ -878,8 +1026,9 @@ tone_alarm_main(int argc, char *argv[]) if (g_dev == nullptr) { g_dev = new ToneAlarm; - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "couldn't allocate the ToneAlarm driver"); + } if (g_dev->init() != OK) { delete g_dev; @@ -894,30 +1043,39 @@ tone_alarm_main(int argc, char *argv[]) play_tune(TONE_STOP_TUNE); } - if (!strcmp(argv1, "stop")) + if (!strcmp(argv1, "stop")) { play_tune(TONE_STOP_TUNE); + } - if ((tune = strtol(argv1, nullptr, 10)) != 0) + if ((tune = strtol(argv1, nullptr, 10)) != 0) { play_tune(tune); + } /* It might be a tune name */ for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) - if (!strcmp(g_dev->name(tune), argv1)) + if (!strcmp(g_dev->name(tune), argv1)) { play_tune(tune); + } /* If it is a file name then load and play it as a string */ if (*argv1 == '/') { FILE *fd = fopen(argv1, "r"); int sz; char *buffer; - if (fd == nullptr) + + if (fd == nullptr) { errx(1, "couldn't open '%s'", argv1); + } + fseek(fd, 0, SEEK_END); sz = ftell(fd); fseek(fd, 0, SEEK_SET); buffer = (char *)malloc(sz + 1); - if (buffer == nullptr) + + if (buffer == nullptr) { errx(1, "not enough memory memory"); + } + fread(buffer, sz, 1, fd); /* terminate the string */ buffer[sz] = 0; From 9d50f1b5f5db47f8768f022d4300b7d47f98c30c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:07:26 +1100 Subject: [PATCH 225/293] trone: update from upstream --- src/drivers/trone/CMakeLists.txt | 44 ++++++++++++++++++++++++ src/drivers/trone/trone.cpp | 59 +++++++++++++++++++------------- 2 files changed, 80 insertions(+), 23 deletions(-) create mode 100644 src/drivers/trone/CMakeLists.txt diff --git a/src/drivers/trone/CMakeLists.txt b/src/drivers/trone/CMakeLists.txt new file mode 100644 index 000000000000..e0166dd333e5 --- /dev/null +++ b/src/drivers/trone/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__trone + MAIN trone + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + trone.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 82122a3ff7ec..e3b77fb19c68 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -125,6 +125,7 @@ class TRONE : public device::I2C work_s _work; ringbuffer::RingBuffer *_reports; bool _sensor_ok; + uint8_t _valid; int _measure_ticks; bool _collect_phase; int _class_instance; @@ -211,7 +212,8 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; -/* static uint8_t crc8(uint8_t *p, uint8_t len) { +static uint8_t crc8(uint8_t *p, uint8_t len) +{ uint16_t i; uint16_t crc = 0x0; @@ -221,7 +223,7 @@ static const uint8_t crc_table[] = { } return crc & 0xFF; -}*/ +} /* * Driver 'main' command. @@ -234,6 +236,7 @@ TRONE::TRONE(int bus, int address) : _max_distance(TRONE_MAX_DISTANCE), _reports(nullptr), _sensor_ok(false), + _valid(0), _measure_ticks(0), _collect_phase(false), _class_instance(-1), @@ -299,10 +302,10 @@ TRONE::init() _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { - log("failed to create distance_sensor object. Did you start uOrb?"); + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -316,17 +319,23 @@ TRONE::init() int TRONE::probe() { - uint8_t who_am_i=0; + uint8_t who_am_i = 0; const uint8_t cmd = TRONE_WHO_AM_I_REG; - if (transfer(&cmd, 1, &who_am_i, 1) == OK && who_am_i == TRONE_WHO_AM_I_REG_VAL) { - // it is responding correctly to a WHO_AM_I - return measure(); + + // set the I2C bus address + set_address(TRONE_BASEADDR); + + // can't use a single transfer as TROne need a bit of time for internal processing + if (transfer(&cmd, 1, nullptr, 0) == OK) { + if (transfer(nullptr, 0, &who_am_i, 1) == OK && who_am_i == TRONE_WHO_AM_I_REG_VAL) { + return measure(); + } } - debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n", - (unsigned)who_am_i, - TRONE_WHO_AM_I_REG_VAL); + DEVICE_DEBUG("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n", + (unsigned)who_am_i, + TRONE_WHO_AM_I_REG_VAL); // not found on any address return -EIO; @@ -543,7 +552,7 @@ TRONE::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + DEVICE_LOG("i2c::transfer returned %d", ret); return ret; } @@ -565,7 +574,7 @@ TRONE::collect() ret = transfer(nullptr, 0, &val[0], 3); if (ret < 0) { - log("error reading from sensor: %d", ret); + DEVICE_LOG("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -586,6 +595,10 @@ TRONE::collect() /* TODO: set proper ID */ report.id = 0; + // This validation check can be used later + _valid = crc8(val, 2) == val[2] && (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0; + /* publish it, if we are the primary */ if (_distance_sensor_topic != nullptr) { orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); @@ -653,7 +666,7 @@ TRONE::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + DEVICE_LOG("collection error"); /* restart the measurement state machine */ start(); return; @@ -668,10 +681,10 @@ TRONE::cycle() 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)); + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL)); return; } @@ -679,7 +692,7 @@ TRONE::cycle() /* measurement phase */ if (OK != measure()) { - log("measure error"); + DEVICE_LOG("measure error"); } /* next phase is collection */ @@ -687,10 +700,10 @@ TRONE::cycle() /* schedule a fresh cycle call when the measurement is done */ work_queue(HPWORK, - &_work, - (worker_t)&TRONE::cycle_trampoline, - this, - USEC2TICK(TRONE_CONVERSION_INTERVAL)); + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + USEC2TICK(TRONE_CONVERSION_INTERVAL)); } void From b75e497bbd43d611c4bfb71637a13a4e5c04064d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:07:40 +1100 Subject: [PATCH 226/293] systemlib: update from upstream --- src/modules/systemlib/CMakeLists.txt | 82 +++ src/modules/systemlib/airspeed.c | 17 +- src/modules/systemlib/airspeed.h | 85 +-- src/modules/systemlib/bson/tinybson.c | 128 ++-- src/modules/systemlib/bson/tinybson.h | 10 +- src/modules/systemlib/circuit_breaker.cpp | 2 +- src/modules/systemlib/circuit_breaker.h | 1 + .../systemlib/circuit_breaker_params.c | 34 +- src/modules/systemlib/cpuload.c | 4 - src/modules/systemlib/cpuload.h | 1 - src/modules/systemlib/err.c | 15 +- src/modules/systemlib/getopt_long.c | 555 +++++++++--------- src/modules/systemlib/getopt_long.h | 37 +- src/modules/systemlib/git_version.h | 4 +- src/modules/systemlib/hx_stream.c | 27 +- src/modules/systemlib/mcu_version.c | 59 +- src/modules/systemlib/mixer/CMakeLists.txt | 55 ++ src/modules/systemlib/mixer/mixer.cpp | 24 +- src/modules/systemlib/mixer/mixer.h | 38 +- src/modules/systemlib/mixer/mixer_load.c | 16 +- .../systemlib/mixer/mixer_multirotor.cpp | 120 ++-- src/modules/systemlib/mixer/mixer_simple.cpp | 42 +- src/modules/systemlib/otp.c | 37 +- src/modules/systemlib/perf_counter.c | 216 ++++--- src/modules/systemlib/ppm_decode.c | 27 +- src/modules/systemlib/print_load_posix.c | 74 +++ src/modules/systemlib/printload.c | 282 +++++++++ src/modules/systemlib/printload.h | 70 +++ src/modules/systemlib/pwm_limit/pwm_limit.c | 232 +++++--- src/modules/systemlib/pwm_limit/pwm_limit.h | 12 +- src/modules/systemlib/rc_check.c | 39 +- src/modules/systemlib/rc_check.h | 2 +- src/modules/systemlib/state_table.h | 14 +- src/modules/systemlib/system_params.c | 13 +- src/modules/systemlib/up_cxxinitialize.c | 34 +- src/modules/systemlib/uthash/uthash.h | 1 + 36 files changed, 1641 insertions(+), 768 deletions(-) create mode 100644 src/modules/systemlib/CMakeLists.txt create mode 100644 src/modules/systemlib/mixer/CMakeLists.txt create mode 100644 src/modules/systemlib/print_load_posix.c create mode 100644 src/modules/systemlib/printload.c create mode 100644 src/modules/systemlib/printload.h diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt new file mode 100644 index 000000000000..59485fe494e2 --- /dev/null +++ b/src/modules/systemlib/CMakeLists.txt @@ -0,0 +1,82 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# for generated files +include_directories(${CMAKE_BINARY_DIR}/src/modules/param) + +set(SRCS + perf_counter.c + param/param.c + conversions.c + cpuload.c + pid/pid.c + airspeed.c + mavlink_log.c + rc_check.c + otp.c + board_serial.c + pwm_limit/pwm_limit.c + mcu_version.c + bson/tinybson.c + circuit_breaker.cpp + ) + +if(${OS} STREQUAL "nuttx") + list(APPEND SRCS + err.c + printload.c + up_cxxinitialize.c + ) +else() + list(APPEND SRCS + print_load_posix.c + ) +endif() + +if(NOT ${OS} STREQUAL "qurt") + list(APPEND SRCS + hx_stream.c + ) +endif() + +px4_add_module( + MODULE modules__systemlib + COMPILE_FLAGS + -Wno-sign-compare + -Os + SRCS ${SRCS} + DEPENDS + platforms__common + modules__param + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 1a479c2058a1..fa5db7c89b59 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -60,13 +60,14 @@ float calc_indicated_airspeed(float differential_pressure) { if (differential_pressure > 0.0f) { - return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return sqrtf((2.0f * differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } else { - return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return -sqrtf((2.0f * fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } } - + /** * Calculate true airspeed from indicated airspeed. * @@ -79,9 +80,10 @@ float calc_indicated_airspeed(float differential_pressure) */ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius) { - return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius)); + return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, + temperature_celsius)); } - + /** * Directly calculate true airspeed * @@ -103,9 +105,10 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp float pressure_difference = total_pressure - static_pressure; if (pressure_difference > 0) { - return sqrtf((2.0f*(pressure_difference)) / density); + return sqrtf((2.0f * (pressure_difference)) / density); + } else { - return -sqrtf((2.0f*fabsf(pressure_difference)) / density); + return -sqrtf((2.0f * fabsf(pressure_difference)) / density); } } diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h index 8dccaab9c57a..2a2b0873da20 100644 --- a/src/modules/systemlib/airspeed.h +++ b/src/modules/systemlib/airspeed.h @@ -46,51 +46,52 @@ #include "math.h" #include "conversions.h" - __BEGIN_DECLS - - /** - * Calculate indicated airspeed. - * - * Note that the indicated airspeed is not the true airspeed because it - * lacks the air density compensation. Use the calc_true_airspeed functions to get - * the true airspeed. - * - * @param total_pressure pressure inside the pitot/prandtl tube - * @param static_pressure pressure at the side of the tube/airplane - * @return indicated airspeed in m/s - */ - __EXPORT float calc_indicated_airspeed(float differential_pressure); - - /** - * Calculate true airspeed from indicated airspeed. - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param speed_indicated current indicated airspeed - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature_celsius air temperature in degrees celcius - * @return true airspeed in m/s - */ - __EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius); - - /** - * Directly calculate true airspeed - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param total_pressure pressure inside the pitot/prandtl tube - * @param static_pressure pressure at the side of the tube/airplane - * @param temperature_celsius air temperature in degrees celcius - * @return true airspeed in m/s - */ - __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); +__BEGIN_DECLS - /** - * Calculates air density. +/** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @return indicated airspeed in m/s + */ +__EXPORT float calc_indicated_airspeed(float differential_pressure); + +/** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param static_pressure ambient pressure in millibar - * @param temperature_celcius air / ambient temperature in celcius + * @param speed_indicated current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s */ +__EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, + float temperature_celsius); + +/** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ +__EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); + +/** +* Calculates air density. +* +* @param static_pressure ambient pressure in millibar +* @param temperature_celcius air / ambient temperature in celcius +*/ __EXPORT float get_air_density(float static_pressure, float temperature_celsius); __END_DECLS diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 2378a2d033b1..e83fd6799995 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -75,14 +75,19 @@ read_x(bson_decoder_t decoder, void *p, size_t s) if (decoder->buf != NULL) { /* staged operations to avoid integer overflow for corrupt data */ - if (s >= decoder->bufsize) + if (s >= decoder->bufsize) { CODER_KILL(decoder, "buffer too small for read"); - if ((decoder->bufsize - s) < decoder->bufpos) + } + + if ((decoder->bufsize - s) < decoder->bufpos) { CODER_KILL(decoder, "not enough data for read"); + } + memcpy(p, (decoder->buf + decoder->bufpos), s); decoder->bufpos += s; return 0; } + debug("no source"); return -1; } @@ -126,31 +131,37 @@ bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback cal decoder->node.type = BSON_UNDEFINED; /* read and discard document size */ - if (read_int32(decoder, &junk)) + if (read_int32(decoder, &junk)) { CODER_KILL(decoder, "failed discarding length"); + } /* ready for decoding */ return 0; } int -bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private) +bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, + void *private) { int32_t len; /* argument sanity */ - if ((buf == NULL) || (callback == NULL)) + if ((buf == NULL) || (callback == NULL)) { return -1; + } decoder->fd = -1; decoder->buf = (uint8_t *)buf; decoder->dead = false; + if (bufsize == 0) { decoder->bufsize = *(uint32_t *)buf; debug("auto-detected %u byte object", decoder->bufsize); + } else { decoder->bufsize = bufsize; } + decoder->bufpos = 0; decoder->callback = callback; decoder->private = private; @@ -159,10 +170,13 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_ decoder->node.type = BSON_UNDEFINED; /* read and discard document size */ - if (read_int32(decoder, &len)) + if (read_int32(decoder, &len)) { CODER_KILL(decoder, "failed reading length"); - if ((len > 0) && (len > (int)decoder->bufsize)) + } + + if ((len > 0) && (len > (int)decoder->bufsize)) { CODER_KILL(decoder, "document length larger than buffer"); + } /* ready for decoding */ return 0; @@ -179,8 +193,9 @@ bson_decoder_next(bson_decoder_t decoder) /* if the previous node was EOO, pop a nesting level */ if (decoder->node.type == BSON_EOO) { - if (decoder->nesting > 0) + if (decoder->nesting > 0) { decoder->nesting--; + } /* if the nesting level is now zero, the top-level document is done */ if (decoder->nesting == 0) { @@ -195,15 +210,17 @@ bson_decoder_next(bson_decoder_t decoder) /* if there are unread bytes pending in the stream, discard them */ while (decoder->pending > 0) { - if (read_int8(decoder, &tbyte)) + if (read_int8(decoder, &tbyte)) { CODER_KILL(decoder, "read error discarding pending bytes"); + } decoder->pending--; } /* get the type byte */ - if (read_int8(decoder, &tbyte)) + if (read_int8(decoder, &tbyte)) { CODER_KILL(decoder, "read error on type byte"); + } decoder->node.type = tbyte; decoder->pending = 0; @@ -213,20 +230,24 @@ bson_decoder_next(bson_decoder_t decoder) /* EOO is special; it has no name/data following */ if (decoder->node.type == BSON_EOO) { decoder->node.name[0] = '\0'; + } else { /* get the node name */ nlen = 0; for (;;) { - if (nlen >= BSON_MAXNAME) + if (nlen >= BSON_MAXNAME) { CODER_KILL(decoder, "node name overflow"); + } - if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) + if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) { CODER_KILL(decoder, "read error on node name"); + } - if (decoder->node.name[nlen] == '\0') + if (decoder->node.name[nlen] == '\0') { break; + } nlen++; } @@ -235,45 +256,55 @@ bson_decoder_next(bson_decoder_t decoder) switch (decoder->node.type) { case BSON_BOOL: - if (read_int8(decoder, &tbyte)) + if (read_int8(decoder, &tbyte)) { CODER_KILL(decoder, "read error on BSON_BOOL"); + } + decoder->node.b = (tbyte != 0); break; case BSON_INT32: - if (read_int32(decoder, &tint)) + if (read_int32(decoder, &tint)) { CODER_KILL(decoder, "read error on BSON_INT"); + } + decoder->node.i = tint; break; case BSON_INT64: - if (read_int64(decoder, &decoder->node.i)) + if (read_int64(decoder, &decoder->node.i)) { CODER_KILL(decoder, "read error on BSON_INT"); + } break; case BSON_DOUBLE: - if (read_double(decoder, &decoder->node.d)) + if (read_double(decoder, &decoder->node.d)) { CODER_KILL(decoder, "read error on BSON_DOUBLE"); + } break; case BSON_STRING: - if (read_int32(decoder, &decoder->pending)) + if (read_int32(decoder, &decoder->pending)) { CODER_KILL(decoder, "read error on BSON_STRING length"); + } + break; case BSON_BINDATA: - if (read_int32(decoder, &decoder->pending)) + if (read_int32(decoder, &decoder->pending)) { CODER_KILL(decoder, "read error on BSON_BINDATA size"); + } - if (read_int8(decoder, &tbyte)) + if (read_int8(decoder, &tbyte)) { CODER_KILL(decoder, "read error on BSON_BINDATA subtype"); + } decoder->node.subtype = tbyte; break; - /* XXX currently not supporting other types */ + /* XXX currently not supporting other types */ default: CODER_KILL(decoder, "unsupported node type"); } @@ -293,8 +324,9 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf) /* copy data */ result = read_x(decoder, buf, decoder->pending); - if (result != 0) + if (result != 0) { CODER_KILL(decoder, "read error on copy_data"); + } /* pending count is discharged */ decoder->pending = 0; @@ -312,17 +344,21 @@ write_x(bson_encoder_t encoder, const void *p, size_t s) { CODER_CHECK(encoder); - if (encoder->fd > -1) + if (encoder->fd > -1) { return (BSON_WRITE(encoder->fd, p, s) == (int)s) ? 0 : -1; + } /* do we need to extend the buffer? */ while ((encoder->bufpos + s) > encoder->bufsize) { - if (!encoder->realloc_ok) + if (!encoder->realloc_ok) { CODER_KILL(encoder, "fixed-size buffer overflow"); + } uint8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT); - if (newbuf == NULL) + + if (newbuf == NULL) { CODER_KILL(encoder, "could not grow buffer"); + } encoder->buf = newbuf; encoder->bufsize += BSON_BUF_INCREMENT; @@ -365,8 +401,9 @@ write_name(bson_encoder_t encoder, const char *name) { size_t len = strlen(name); - if (len > BSON_MAXNAME) + if (len > BSON_MAXNAME) { CODER_KILL(encoder, "node name too long"); + } return write_x(encoder, name, len + 1); } @@ -378,8 +415,9 @@ bson_encoder_init_file(bson_encoder_t encoder, int fd) encoder->buf = NULL; encoder->dead = false; - if (write_int32(encoder, 0)) + if (write_int32(encoder, 0)) { CODER_KILL(encoder, "write error on document length"); + } return 0; } @@ -391,18 +429,21 @@ bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize) encoder->buf = (uint8_t *)buf; encoder->bufpos = 0; encoder->dead = false; + if (encoder->buf == NULL) { encoder->bufsize = 0; encoder->realloc_ok = true; + } else { encoder->bufsize = bufsize; encoder->realloc_ok = false; } - if (write_int32(encoder, 0)) + if (write_int32(encoder, 0)) { CODER_KILL(encoder, "write error on document length"); + } - return 0; + return 0; } int @@ -410,8 +451,9 @@ bson_encoder_fini(bson_encoder_t encoder) { CODER_CHECK(encoder); - if (write_int8(encoder, BSON_EOO)) + if (write_int8(encoder, BSON_EOO)) { CODER_KILL(encoder, "write error on document terminator"); + } /* hack to fix up length for in-buffer documents */ if (encoder->buf != NULL) { @@ -430,8 +472,9 @@ bson_encoder_buf_size(bson_encoder_t encoder) { CODER_CHECK(encoder); - if (encoder->fd > -1) + if (encoder->fd > -1) { return -1; + } return encoder->bufpos; } @@ -441,8 +484,9 @@ bson_encoder_buf_data(bson_encoder_t encoder) { /* note, no CODER_CHECK here as the caller has to clean up dead buffers */ - if (encoder->fd > -1) + if (encoder->fd > -1) { return NULL; + } return encoder->buf; } @@ -453,8 +497,9 @@ int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool valu if (write_int8(encoder, BSON_BOOL) || write_name(encoder, name) || - write_int8(encoder, value ? 1 : 0)) + write_int8(encoder, value ? 1 : 0)) { CODER_KILL(encoder, "write error on BSON_BOOL"); + } return 0; } @@ -472,14 +517,17 @@ bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value) result = write_int8(encoder, BSON_INT32) || write_name(encoder, name) || write_int32(encoder, value); + } else { debug("encoding %lld as int64", value); result = write_int8(encoder, BSON_INT64) || write_name(encoder, name) || write_int64(encoder, value); } - if (result) + + if (result) { CODER_KILL(encoder, "write error on BSON_INT"); + } return 0; } @@ -491,8 +539,9 @@ bson_encoder_append_double(bson_encoder_t encoder, const char *name, double valu if (write_int8(encoder, BSON_DOUBLE) || write_name(encoder, name) || - write_double(encoder, value)) + write_double(encoder, value)) { CODER_KILL(encoder, "write error on BSON_DOUBLE"); + } return 0; @@ -510,14 +559,16 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char if (write_int8(encoder, BSON_STRING) || write_name(encoder, name) || write_int32(encoder, len) || - write_x(encoder, string, len)) + write_x(encoder, string, len)) { CODER_KILL(encoder, "write error on BSON_STRING"); + } return 0; } int -bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data) +bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, + const void *data) { CODER_CHECK(encoder); @@ -525,8 +576,9 @@ bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary write_name(encoder, name) || write_int32(encoder, size) || write_int8(encoder, subtype) || - write_x(encoder, data, size)) + write_x(encoder, data, size)) { CODER_KILL(encoder, "write error on BSON_BINDATA"); + } return 0; } diff --git a/src/modules/systemlib/bson/tinybson.h b/src/modules/systemlib/bson/tinybson.h index 666f8191aa9d..25bd90a9eb98 100644 --- a/src/modules/systemlib/bson/tinybson.h +++ b/src/modules/systemlib/bson/tinybson.h @@ -141,7 +141,8 @@ __EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder * @param private Callback private data, stored in node. * @return Zero on success. */ -__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private); +__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, + void *private); /** * Process the next node from the stream and invoke the callback. @@ -198,7 +199,7 @@ __EXPORT int bson_encoder_init_file(bson_encoder_t encoder, int fd); * @param encoder Encoder state structure to be initialised. * @param buf Buffer pointer to use, or NULL if the buffer * should be allocated by the encoder. - * @param bufsize Maximum buffer size, or zero for no limit. If + * @param bufsize Maximum buffer size, or zero for no limit. If * the buffer is supplied, the size of the supplied buffer. * @return Zero on success. */ @@ -238,7 +239,7 @@ __EXPORT int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, * * @param encoder Encoder state. * @param name Node name. - * @param value Value to be encoded. + * @param value Value to be encoded. */ __EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value); @@ -269,7 +270,8 @@ __EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name * @param size Data size. * @param data Buffer containing data to be encoded. */ -__EXPORT int bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data); +__EXPORT int bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, + size_t size, const void *data); #endif diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index 5d9c7ec3716e..ba9e9f62197c 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -48,7 +48,7 @@ bool circuit_breaker_enabled(const char *breaker, int32_t magic) { - int32_t val = 0; + int32_t val; (void)PX4_PARAM_GET_BYNAME(breaker, &val); return (val == magic); diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index c76e6c37f383..b4d9b5d1c6b0 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -56,6 +56,7 @@ #define CBRK_FLIGHTTERM_KEY 121212 #define CBRK_ENGINEFAIL_KEY 284953 #define CBRK_GPSFAIL_KEY 240024 +#define CBRK_USB_CHK_KEY 197848 #include diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index 097903d21f18..a3eaebbb33bb 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -42,8 +42,6 @@ * parameter needs to set to the key (magic). */ -#include - /** * Circuit breaker for power supply check * @@ -73,7 +71,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); /** * Circuit breaker for IO safety * - * Setting this parameter to 894281 will disable IO safety. + * Setting this parameter to 22027 will disable IO safety. * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK * * @min 0 @@ -128,11 +126,37 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); * If this check is enabled, then the sensor check will fail if the GPS module * is missing. It will also check for excessive signal noise on the GPS receiver * and warn the user if detected. - * + * * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK * * @min 0 * @max 240024 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); \ No newline at end of file +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); + +/** + * Circuit breaker for disabling buzzer + * + * Setting this parameter to 782097 will disable the buzzer audio notification. + * + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 782097 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_BUZZER, 0); + +/** + * Circuit breaker for USB link check + * + * Setting this parameter to 197848 will disable the USB connected + * checks in the commander. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 197848 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_USB_CHK, 0); diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index fe08da71fd77..77743bdd8c5a 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -80,8 +80,6 @@ void cpuload_initialize_once() system_load.tasks[i].valid = false; } - uint64_t now = hrt_absolute_time(); - int static_tasks_count = 2; // there are at least 2 threads that should be initialized statically - "idle" and "init" #ifdef CONFIG_PAGING @@ -96,7 +94,6 @@ void cpuload_initialize_once() // perform static initialization of "system" threads for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) { - system_load.tasks[system_load.total_count].start_time = now; system_load.tasks[system_load.total_count].total_runtime = 0; system_load.tasks[system_load.total_count].curr_start_time = 0; system_load.tasks[system_load.total_count].tcb = sched_gettcb( @@ -113,7 +110,6 @@ void sched_note_start(FAR struct tcb_s *tcb) for (i = 1; i < CONFIG_MAX_TASKS; i++) { if (!system_load.tasks[i].valid) { /* slot is available */ - system_load.tasks[i].start_time = hrt_absolute_time(); system_load.tasks[i].total_runtime = 0; system_load.tasks[i].curr_start_time = 0; system_load.tasks[i].tcb = tcb; diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h index dc173baa797c..e997bb3d9ab2 100644 --- a/src/modules/systemlib/cpuload.h +++ b/src/modules/systemlib/cpuload.h @@ -42,7 +42,6 @@ __BEGIN_DECLS struct system_load_taskinfo_s { uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load uint64_t curr_start_time; ///< Start time of the current scheduling slot - uint64_t start_time; ///< FIRST start time of task #ifdef __PX4_NUTTX FAR struct tcb_s *tcb; ///< #endif diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 3e1bdd0b19f0..b7b6f0a9c3d7 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -40,6 +40,9 @@ #include +#define __STDC_FORMAT_MACROS +#include + #include #include #include @@ -77,11 +80,13 @@ warnerr_core(int errcode, const char *fmt, va_list args) vfprintf(stderr, fmt, args); /* convenience as many parts of NuttX use negative errno */ - if (errcode < 0) + if (errcode < 0) { errcode = -errcode; + } - if (errcode < NOCODE) + if (errcode < NOCODE) { fprintf(stderr, ": %s", strerror(errcode)); + } fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC @@ -89,11 +94,13 @@ warnerr_core(int errcode, const char *fmt, va_list args) lowvsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ - if (errcode < 0) + if (errcode < 0) { errcode = -errcode; + } - if (errcode < NOCODE) + if (errcode < NOCODE) { lowsyslog(": %s", strerror(errcode)); + } lowsyslog("\n"); #endif diff --git a/src/modules/systemlib/getopt_long.c b/src/modules/systemlib/getopt_long.c index 27c38635f7b7..c715221a0093 100644 --- a/src/modules/systemlib/getopt_long.c +++ b/src/modules/systemlib/getopt_long.c @@ -75,7 +75,7 @@ COPYRIGHT NOTICE AND DISCLAIMER: Copyright (C) 1997 Gregory Pietsch -This file and the accompanying getopt.h header file are hereby placed in the +This file and the accompanying getopt.h header file are hereby placed in the public domain without restrictions. Just give the author credit, don't claim you wrote it or prevent anyone else from using it. @@ -92,11 +92,10 @@ gpietsch@comcast.net /* macros */ /* types */ -typedef enum GETOPT_ORDERING_T -{ - PERMUTE, - RETURN_IN_ORDER, - REQUIRE_ORDER +typedef enum GETOPT_ORDERING_T { + PERMUTE, + RETURN_IN_ORDER, + REQUIRE_ORDER } GETOPT_ORDERING_T; /* globally-defined variables */ @@ -109,296 +108,320 @@ int optopt = '?'; /* reverse_argv_elements: reverses num elements starting at argv */ static void -reverse_argv_elements (char **argv, int num) +reverse_argv_elements(char **argv, int num) { - int i; - char *tmp; - - for (i = 0; i < (num >> 1); i++) - { - tmp = argv[i]; - argv[i] = argv[num - i - 1]; - argv[num - i - 1] = tmp; - } + int i; + char *tmp; + + for (i = 0; i < (num >> 1); i++) { + tmp = argv[i]; + argv[i] = argv[num - i - 1]; + argv[num - i - 1] = tmp; + } } /* permute: swap two blocks of argv-elements given their lengths */ static void -permute (char **argv, int len1, int len2) +permute(char **argv, int len1, int len2) { - reverse_argv_elements (argv, len1); - reverse_argv_elements (argv, len1 + len2); - reverse_argv_elements (argv, len2); + reverse_argv_elements(argv, len1); + reverse_argv_elements(argv, len1 + len2); + reverse_argv_elements(argv, len2); } /* is_option: is this argv-element an option or the end of the option list? */ static int -is_option (char *argv_element, int only) +is_option(char *argv_element, int only) { - return ((argv_element == NULL) - || (argv_element[0] == '-') || (only && argv_element[0] == '+')); + return ((argv_element == NULL) + || (argv_element[0] == '-') || (only && argv_element[0] == '+')); } /* getopt_internal: the function that does all the dirty work */ static int -getopt_internal (int argc, char **argv, const char *shortopts, - const GETOPT_LONG_OPTION_T * longopts, int *longind, int only) +getopt_internal(int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T *longopts, int *longind, int only) { - GETOPT_ORDERING_T ordering = PERMUTE; - static size_t optwhere = 0; - size_t permute_from = 0; - int num_nonopts = 0; - int optindex = 0; - size_t match_chars = 0; - char *possible_arg = NULL; - int longopt_match = -1; - int has_arg = -1; - char *cp = NULL; - int arg_next = 0; - - /* first, deal with silly parameters and easy stuff */ - if (argc == 0 || argv == NULL || (shortopts == NULL && longopts == NULL) - || optind >= argc || argv[optind] == NULL) - return EOF; - if (strcmp (argv[optind], "--") == 0) - { - optind++; - return EOF; - } - /* if this is our first time through */ - if (optind == 0) - optind = optwhere = 1; - - /* define ordering */ - if (shortopts != NULL && (*shortopts == '-' || *shortopts == '+')) - { - ordering = (*shortopts == '-') ? RETURN_IN_ORDER : REQUIRE_ORDER; - shortopts++; - } - else - ordering = /*(getenv ("POSIXLY_CORRECT") != NULL) ? REQUIRE_ORDER :*/ PERMUTE; - - /* - * based on ordering, find our next option, if we're at the beginning of - * one - */ - if (optwhere == 1) - { - switch (ordering) - { - default: /* shouldn't happen */ - case PERMUTE: - permute_from = optind; - num_nonopts = 0; - while (!is_option (argv[optind], only)) - { - optind++; - num_nonopts++; - } - if (argv[optind] == NULL) - { - /* no more options */ - optind = permute_from; - return EOF; - } - else if (strcmp (argv[optind], "--") == 0) - { - /* no more options, but have to get `--' out of the way */ - permute (argv + permute_from, num_nonopts, 1); - optind = permute_from + 1; - return EOF; - } - break; - case RETURN_IN_ORDER: - if (!is_option (argv[optind], only)) - { - optarg = argv[optind++]; - return (optopt = 1); - } - break; - case REQUIRE_ORDER: - if (!is_option (argv[optind], only)) - return EOF; - break; - } - } - /* we've got an option, so parse it */ - - /* first, is it a long option? */ - if (longopts != NULL - && (memcmp (argv[optind], "--", 2) == 0 - || (only && argv[optind][0] == '+')) && optwhere == 1) - { - /* handle long options */ - if (memcmp (argv[optind], "--", 2) == 0) - optwhere = 2; - longopt_match = -1; - possible_arg = strchr (argv[optind] + optwhere, '='); - if (possible_arg == NULL) - { - /* no =, so next argv might be arg */ - match_chars = strlen (argv[optind]); - possible_arg = argv[optind] + match_chars; - match_chars = match_chars - optwhere; - } - else - match_chars = (possible_arg - argv[optind]) - optwhere; - for (optindex = 0; longopts[optindex].name != NULL; optindex++) - { - if (memcmp (argv[optind] + optwhere, - longopts[optindex].name, match_chars) == 0) - { - /* do we have an exact match? */ - if (match_chars == (unsigned) (strlen (longopts[optindex].name))) - { - longopt_match = optindex; - break; - } - /* do any characters match? */ - else - { - if (longopt_match < 0) - longopt_match = optindex; - else - { - /* we have ambiguous options */ - if (opterr) - fprintf (stderr, "%s: option `%s' is ambiguous " - "(could be `--%s' or `--%s')\n", - argv[0], - argv[optind], - longopts[longopt_match].name, - longopts[optindex].name); - return (optopt = '?'); - } - } - } - } - if (longopt_match >= 0) - has_arg = longopts[longopt_match].has_arg; - } - /* if we didn't find a long option, is it a short option? */ - if (longopt_match < 0 && shortopts != NULL) - { - cp = strchr (shortopts, argv[optind][optwhere]); - if (cp == NULL) - { - /* couldn't find option in shortopts */ - if (opterr) - fprintf (stderr, - "%s: invalid option -- `-%c'\n", - argv[0], argv[optind][optwhere]); - optwhere++; - if (argv[optind][optwhere] == '\0') - { - optind++; - optwhere = 1; - } - return (optopt = '?'); - } - has_arg = ((cp[1] == ':') - ? ((cp[2] == ':') ? OPTIONAL_ARG : REQUIRED_ARG) : NO_ARG); - possible_arg = argv[optind] + optwhere + 1; - optopt = *cp; - } - /* get argument and reset optwhere */ - arg_next = 0; - switch (has_arg) - { - case OPTIONAL_ARG: - if (*possible_arg == '=') - possible_arg++; - optarg = (*possible_arg != '\0') ? possible_arg : 0; - optwhere = 1; - break; - case REQUIRED_ARG: - if (*possible_arg == '=') - possible_arg++; - if (*possible_arg != '\0') - { - optarg = possible_arg; - optwhere = 1; - } - else if (optind + 1 >= argc) - { - if (opterr) - { - fprintf (stderr, "%s: argument required for option `", argv[0]); - if (longopt_match >= 0) - fprintf (stderr, "--%s'\n", longopts[longopt_match].name); - else - fprintf (stderr, "-%c'\n", *cp); - } - optind++; - return (optopt = ':'); - } - else - { - optarg = argv[optind + 1]; - arg_next = 1; - optwhere = 1; - } - break; - default: /* shouldn't happen */ - case NO_ARG: - if (longopt_match < 0) - { - optwhere++; - if (argv[optind][optwhere] == '\0') - optwhere = 1; - } - else - optwhere = 1; - optarg = NULL; - break; - } - - /* do we have to permute or otherwise modify optind? */ - if (ordering == PERMUTE && optwhere == 1 && num_nonopts != 0) - { - permute (argv + permute_from, num_nonopts, 1 + arg_next); - optind = permute_from + 1 + arg_next; - } - else if (optwhere == 1) - optind = optind + 1 + arg_next; - - /* finally return */ - if (longopt_match >= 0) - { - if (longind != NULL) - *longind = longopt_match; - if (longopts[longopt_match].flag != NULL) - { - *(longopts[longopt_match].flag) = longopts[longopt_match].val; - return 0; - } - else - return longopts[longopt_match].val; - } - else - return optopt; + GETOPT_ORDERING_T ordering = PERMUTE; + static size_t optwhere = 0; + size_t permute_from = 0; + int num_nonopts = 0; + int optindex = 0; + size_t match_chars = 0; + char *possible_arg = NULL; + int longopt_match = -1; + int has_arg = -1; + char *cp = NULL; + int arg_next = 0; + + /* first, deal with silly parameters and easy stuff */ + if (argc == 0 || argv == NULL || (shortopts == NULL && longopts == NULL) + || optind >= argc || argv[optind] == NULL) { + return EOF; + } + + if (strcmp(argv[optind], "--") == 0) { + optind++; + return EOF; + } + + /* if this is our first time through */ + if (optind == 0) { + optind = optwhere = 1; + } + + /* define ordering */ + if (shortopts != NULL && (*shortopts == '-' || *shortopts == '+')) { + ordering = (*shortopts == '-') ? RETURN_IN_ORDER : REQUIRE_ORDER; + shortopts++; + + } else { + ordering = /*(getenv ("POSIXLY_CORRECT") != NULL) ? REQUIRE_ORDER :*/ PERMUTE; + } + + /* + * based on ordering, find our next option, if we're at the beginning of + * one + */ + if (optwhere == 1) { + switch (ordering) { + default: /* shouldn't happen */ + case PERMUTE: + permute_from = optind; + num_nonopts = 0; + + while (!is_option(argv[optind], only)) { + optind++; + num_nonopts++; + } + + if (argv[optind] == NULL) { + /* no more options */ + optind = permute_from; + return EOF; + + } else if (strcmp(argv[optind], "--") == 0) { + /* no more options, but have to get `--' out of the way */ + permute(argv + permute_from, num_nonopts, 1); + optind = permute_from + 1; + return EOF; + } + + break; + + case RETURN_IN_ORDER: + if (!is_option(argv[optind], only)) { + optarg = argv[optind++]; + return (optopt = 1); + } + + break; + + case REQUIRE_ORDER: + if (!is_option(argv[optind], only)) { + return EOF; + } + + break; + } + } + + /* we've got an option, so parse it */ + + /* first, is it a long option? */ + if (longopts != NULL + && (memcmp(argv[optind], "--", 2) == 0 + || (only && argv[optind][0] == '+')) && optwhere == 1) { + /* handle long options */ + if (memcmp(argv[optind], "--", 2) == 0) { + optwhere = 2; + } + + longopt_match = -1; + possible_arg = strchr(argv[optind] + optwhere, '='); + + if (possible_arg == NULL) { + /* no =, so next argv might be arg */ + match_chars = strlen(argv[optind]); + possible_arg = argv[optind] + match_chars; + match_chars = match_chars - optwhere; + + } else { + match_chars = (possible_arg - argv[optind]) - optwhere; + } + + for (optindex = 0; longopts[optindex].name != NULL; optindex++) { + if (memcmp(argv[optind] + optwhere, + longopts[optindex].name, match_chars) == 0) { + /* do we have an exact match? */ + if (match_chars == (unsigned)(strlen(longopts[optindex].name))) { + longopt_match = optindex; + break; + } + + /* do any characters match? */ + else { + if (longopt_match < 0) { + longopt_match = optindex; + + } else { + /* we have ambiguous options */ + if (opterr) + fprintf(stderr, "%s: option `%s' is ambiguous " + "(could be `--%s' or `--%s')\n", + argv[0], + argv[optind], + longopts[longopt_match].name, + longopts[optindex].name); + + return (optopt = '?'); + } + } + } + } + + if (longopt_match >= 0) { + has_arg = longopts[longopt_match].has_arg; + } + } + + /* if we didn't find a long option, is it a short option? */ + if (longopt_match < 0 && shortopts != NULL) { + cp = strchr(shortopts, argv[optind][optwhere]); + + if (cp == NULL) { + /* couldn't find option in shortopts */ + if (opterr) + fprintf(stderr, + "%s: invalid option -- `-%c'\n", + argv[0], argv[optind][optwhere]); + + optwhere++; + + if (argv[optind][optwhere] == '\0') { + optind++; + optwhere = 1; + } + + return (optopt = '?'); + } + + has_arg = ((cp[1] == ':') + ? ((cp[2] == ':') ? OPTIONAL_ARG : REQUIRED_ARG) : NO_ARG); + possible_arg = argv[optind] + optwhere + 1; + optopt = *cp; + } + + /* get argument and reset optwhere */ + arg_next = 0; + + switch (has_arg) { + case OPTIONAL_ARG: + if (*possible_arg == '=') { + possible_arg++; + } + + optarg = (*possible_arg != '\0') ? possible_arg : 0; + optwhere = 1; + break; + + case REQUIRED_ARG: + if (*possible_arg == '=') { + possible_arg++; + } + + if (*possible_arg != '\0') { + optarg = possible_arg; + optwhere = 1; + + } else if (optind + 1 >= argc) { + if (opterr) { + fprintf(stderr, "%s: argument required for option `", argv[0]); + + if (longopt_match >= 0) { + fprintf(stderr, "--%s'\n", longopts[longopt_match].name); + + } else { + fprintf(stderr, "-%c'\n", *cp); + } + } + + optind++; + return (optopt = ':'); + + } else { + optarg = argv[optind + 1]; + arg_next = 1; + optwhere = 1; + } + + break; + + default: /* shouldn't happen */ + case NO_ARG: + if (longopt_match < 0) { + optwhere++; + + if (argv[optind][optwhere] == '\0') { + optwhere = 1; + } + + } else { + optwhere = 1; + } + + optarg = NULL; + break; + } + + /* do we have to permute or otherwise modify optind? */ + if (ordering == PERMUTE && optwhere == 1 && num_nonopts != 0) { + permute(argv + permute_from, num_nonopts, 1 + arg_next); + optind = permute_from + 1 + arg_next; + + } else if (optwhere == 1) { + optind = optind + 1 + arg_next; + } + + /* finally return */ + if (longopt_match >= 0) { + if (longind != NULL) { + *longind = longopt_match; + } + + if (longopts[longopt_match].flag != NULL) { + *(longopts[longopt_match].flag) = longopts[longopt_match].val; + return 0; + + } else { + return longopts[longopt_match].val; + } + + } else { + return optopt; + } } #if 0 int -getopt (int argc, char **argv, char *optstring) +getopt(int argc, char **argv, char *optstring) { - return getopt_internal (argc, argv, optstring, NULL, NULL, 0); + return getopt_internal(argc, argv, optstring, NULL, NULL, 0); } #endif int -getopt_long (int argc, char **argv, const char *shortopts, - const GETOPT_LONG_OPTION_T * longopts, int *longind) +getopt_long(int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T *longopts, int *longind) { - return getopt_internal (argc, argv, shortopts, longopts, longind, 0); + return getopt_internal(argc, argv, shortopts, longopts, longind, 0); } int -getopt_long_only (int argc, char **argv, const char *shortopts, - const GETOPT_LONG_OPTION_T * longopts, int *longind) +getopt_long_only(int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T *longopts, int *longind) { - return getopt_internal (argc, argv, shortopts, longopts, longind, 1); + return getopt_internal(argc, argv, shortopts, longopts, longind, 1); } /* end of file GETOPT.C */ diff --git a/src/modules/systemlib/getopt_long.h b/src/modules/systemlib/getopt_long.h index 61e8e76f3146..ead667c73a28 100644 --- a/src/modules/systemlib/getopt_long.h +++ b/src/modules/systemlib/getopt_long.h @@ -74,8 +74,8 @@ COPYRIGHT NOTICE AND DISCLAIMER: Copyright (C) 1997 Gregory Pietsch -This file and the accompanying getopt.c implementation file are hereby -placed in the public domain without restrictions. Just give the author +This file and the accompanying getopt.c implementation file are hereby +placed in the public domain without restrictions. Just give the author credit, don't claim you wrote it or prevent anyone else from using it. Gregory Pietsch's current e-mail address: @@ -95,17 +95,16 @@ gpietsch@comcast.net /* types defined by this include file */ /* GETOPT_LONG_OPTION_T: The type of long option */ -typedef struct GETOPT_LONG_OPTION_T -{ - char *name; /* the name of the long option */ - int has_arg; /* one of the above macros */ - int *flag; /* determines if getopt_long() returns a +typedef struct GETOPT_LONG_OPTION_T { + char *name; /* the name of the long option */ + int has_arg; /* one of the above macros */ + int *flag; /* determines if getopt_long() returns a * value for a long option; if it is * non-NULL, 0 is returned as a function * value and the value of val is stored in * the area pointed to by flag. Otherwise, * val is returned. */ - int val; /* determines the value to return if flag is + int val; /* determines the value to return if flag is * NULL. */ } GETOPT_LONG_OPTION_T; @@ -114,20 +113,20 @@ extern "C" { #endif - /* externally-defined variables */ - extern char *optarg; - extern int optind; - extern int opterr; - extern int optopt; +/* externally-defined variables */ +extern char *optarg; +extern int optind; +extern int opterr; +extern int optopt; - /* function prototypes */ +/* function prototypes */ #if 0 - int getopt (int argc, char **argv, char *optstring); +int getopt(int argc, char **argv, char *optstring); #endif - __EXPORT int getopt_long (int argc, char **argv, const char *shortopts, - const GETOPT_LONG_OPTION_T * longopts, int *longind); - __EXPORT int getopt_long_only (int argc, char **argv, const char *shortopts, - const GETOPT_LONG_OPTION_T * longopts, int *longind); +__EXPORT int getopt_long(int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T *longopts, int *longind); +__EXPORT int getopt_long_only(int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T *longopts, int *longind); #ifdef __cplusplus }; diff --git a/src/modules/systemlib/git_version.h b/src/modules/systemlib/git_version.h index 9b8f9e860ef0..63674800f8a3 100644 --- a/src/modules/systemlib/git_version.h +++ b/src/modules/systemlib/git_version.h @@ -41,9 +41,11 @@ #include +// #include "build_git_version.h" + __BEGIN_DECLS -__EXPORT extern const char* px4_git_version; +__EXPORT extern const char *px4_git_version; __EXPORT extern const uint64_t px4_git_version_binary; __END_DECLS diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 52ae77de558d..8f388e40783d 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -92,8 +92,9 @@ static int hx_rx_frame(hx_stream_t stream); static void hx_tx_raw(hx_stream_t stream, uint8_t c) { - if (write(stream->fd, &c, 1) != 1) + if (write(stream->fd, &c, 1) != 1) { stream->tx_error = true; + } } static int @@ -111,8 +112,9 @@ hx_rx_frame(hx_stream_t stream) /* not a real frame - too short */ if (length < 4) { - if (length > 1) + if (length > 1) { perf_count(stream->pc_rx_errors); + } return 0; } @@ -190,11 +192,12 @@ hx_stream_reset(hx_stream_t stream) int hx_stream_start(hx_stream_t stream, - const void *data, - size_t count) + const void *data, + size_t count) { - if (count > HX_STREAM_MAX_FRAME) + if (count > HX_STREAM_MAX_FRAME) { return -EINVAL; + } stream->tx_buf = data; stream->tx_resid = count; @@ -224,6 +227,7 @@ hx_stream_send_next(hx_stream_t stream) stream->tx_state = TX_SENT_ESCAPE; return CEO; } + break; case TX_SENT_ESCAPE: @@ -251,12 +255,14 @@ hx_stream_send_next(hx_stream_t stream) /* was the buffer the frame CRC? */ if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) { stream->tx_state = TX_SEND_END; + } else { /* no, it was the payload - switch to sending the CRC */ stream->tx_buf = pcrc; stream->tx_resid = sizeof(stream->tx_crc); } } + return c; } @@ -268,12 +274,16 @@ hx_stream_send(hx_stream_t stream, int result; result = hx_stream_start(stream, data, count); - if (result != OK) + + if (result != OK) { return result; + } int c; - while ((c = hx_stream_send_next(stream)) >= 0) + + while ((c = hx_stream_send_next(stream)) >= 0) { hx_tx_raw(stream, c); + } /* check for transmit error */ if (stream->tx_error) { @@ -306,6 +316,7 @@ hx_stream_rx(hx_stream_t stream, uint8_t c) } /* save for later */ - if (stream->rx_frame_bytes < sizeof(stream->rx_buf)) + if (stream->rx_frame_bytes < sizeof(stream->rx_buf)) { stream->rx_buf[stream->rx_frame_bytes++] = c; + } } diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 1f66971c153e..be614f7d7a8e 100644 --- a/src/modules/systemlib/mcu_version.c +++ b/src/modules/systemlib/mcu_version.c @@ -33,7 +33,7 @@ /** * @file mcu_version.c - * + * * Read out the microcontroller version from the board * * @author Lorenz Meier @@ -62,12 +62,18 @@ /** Copy the 96bit MCU Unique ID into the provided pointer */ void mcu_unique_id(uint32_t *uid_96_bit) { +#ifdef CONFIG_ARCH_CHIP_STM32 uid_96_bit[0] = getreg32(UNIQUE_ID); - uid_96_bit[1] = getreg32(UNIQUE_ID+4); - uid_96_bit[2] = getreg32(UNIQUE_ID+8); + uid_96_bit[1] = getreg32(UNIQUE_ID + 4); + uid_96_bit[2] = getreg32(UNIQUE_ID + 8); +#else + uid_96_bit[0] = 0; + uid_96_bit[1] = 1; + uid_96_bit[2] = 2; +#endif } -int mcu_version(char* rev, char** revstr) +int mcu_version(char *rev, char **revstr) { #ifdef CONFIG_ARCH_CHIP_STM32 uint32_t abc = getreg32(DBGMCU_IDCODE); @@ -79,9 +85,11 @@ int mcu_version(char* rev, char** revstr) case STM32F40x_41x: *revstr = "STM32F40x"; break; + case STM32F42x_43x: *revstr = "STM32F42x"; break; + default: *revstr = "STM32F???"; break; @@ -89,25 +97,30 @@ int mcu_version(char* rev, char** revstr) 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; + 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; diff --git a/src/modules/systemlib/mixer/CMakeLists.txt b/src/modules/systemlib/mixer/CMakeLists.txt new file mode 100644 index 000000000000..20187eb97fe6 --- /dev/null +++ b/src/modules/systemlib/mixer/CMakeLists.txt @@ -0,0 +1,55 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include_directories(${CMAKE_CURRENT_BINARY_DIR}) + +add_custom_command(OUTPUT mixer_multirotor.generated.h + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/multi_tables.py + > mixer_multirotor.generated.h) + +add_custom_target(mixer_gen + DEPENDS mixer_multirotor.generated.h) + +px4_add_module( + MODULE modules__systemlib__mixer + SRCS + mixer.cpp + mixer_group.cpp + mixer_multirotor.cpp + mixer_simple.cpp + mixer_load.c + mixer_multirotor.generated.h + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index 6d5bcd4857cf..982b7db82b88 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -98,20 +98,25 @@ Mixer::scale(const mixer_scaler_s &scaler, float input) int Mixer::scale_check(struct mixer_scaler_s &scaler) { - if (scaler.offset > 1.001f) + if (scaler.offset > 1.001f) { return 1; + } - if (scaler.offset < -1.001f) + if (scaler.offset < -1.001f) { return 2; + } - if (scaler.min_output > scaler.max_output) + if (scaler.min_output > scaler.max_output) { return 3; + } - if (scaler.min_output < -1.001f) + if (scaler.min_output < -1.001f) { return 4; + } - if (scaler.max_output > 1.001f) + if (scaler.max_output > 1.001f) { return 5; + } return 0; } @@ -120,11 +125,14 @@ const char * Mixer::findtag(const char *buf, unsigned &buflen, char tag) { while (buflen >= 2) { - if ((buf[0] == tag) && (buf[1] == ':')) + if ((buf[0] == tag) && (buf[1] == ':')) { return buf; + } + buf++; buflen--; } + return nullptr; } @@ -174,13 +182,15 @@ NullMixer::from_text(const char *buf, unsigned &buflen) /* enforce that the mixer ends with space or a new line */ for (int i = buflen - 1; i >= 0; i--) { - if (buf[i] == '\0') + if (buf[i] == '\0') { continue; + } /* require a space or newline at the end of the buffer, fail on printable chars */ if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { /* found a line ending or space, so no split symbols / numbers. good. */ break; + } else { return nm; } diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index cc3342a5728a..5ed152127a84 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -222,7 +222,7 @@ class __EXPORT Mixer * @param buflen length of the buffer. * @param tag character to search for. */ - static const char * findtag(const char *buf, unsigned &buflen, char tag); + static const char *findtag(const char *buf, unsigned &buflen, char tag); /** * Skip a line @@ -231,13 +231,13 @@ class __EXPORT Mixer * @param buflen length of the buffer. * @return 0 / OK if a line could be skipped, 1 else */ - static const char * skipline(const char *buf, unsigned &buflen); + static const char *skipline(const char *buf, unsigned &buflen); private: /* do not allow to copy due to prt data members */ - Mixer(const Mixer&); - Mixer& operator=(const Mixer&); + Mixer(const Mixer &); + Mixer &operator=(const Mixer &); }; /** @@ -316,8 +316,8 @@ class __EXPORT MixerGroup : public Mixer Mixer *_first; /**< linked list of mixers */ /* do not allow to copy due to pointer data members */ - MixerGroup(const MixerGroup&); - MixerGroup operator=(const MixerGroup&); + MixerGroup(const MixerGroup &); + MixerGroup operator=(const MixerGroup &); }; /** @@ -437,8 +437,8 @@ class __EXPORT SimpleMixer : public Mixer uint8_t &control_index); /* do not allow to copy due to ptr data members */ - SimpleMixer(const SimpleMixer&); - SimpleMixer operator=(const SimpleMixer&); + SimpleMixer(const SimpleMixer &); + SimpleMixer operator=(const SimpleMixer &); }; /** @@ -449,13 +449,13 @@ class __EXPORT SimpleMixer : public Mixer typedef unsigned int MultirotorGeometryUnderlyingType; enum class MultirotorGeometry : MultirotorGeometryUnderlyingType; -/** - * Multi-rotor mixer for pre-defined vehicle geometries. - * - * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to - * a set of outputs based on the configured geometry. - */ -class __EXPORT MultirotorMixer : public Mixer + /** + * Multi-rotor mixer for pre-defined vehicle geometries. + * + * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to + * a set of outputs based on the configured geometry. + */ + class __EXPORT MultirotorMixer : public Mixer { public: /** @@ -481,7 +481,7 @@ class __EXPORT MultirotorMixer : public Mixer * compared to thrust. * @param yaw_wcale Scaling factor applied to yaw inputs compared * to thrust. - * @param deadband Minumum rotor control output value; usually + * @param idle_speed Minumum rotor control output value; usually * tuned to ensure that rotors never stall at the * low end of their control range. */ @@ -491,7 +491,7 @@ class __EXPORT MultirotorMixer : public Mixer float roll_scale, float pitch_scale, float yaw_scale, - float deadband); + float idle_speed); ~MultirotorMixer(); /** @@ -531,8 +531,8 @@ class __EXPORT MultirotorMixer : public Mixer const Rotor *_rotors; /* do not allow to copy due to ptr data members */ - MultirotorMixer(const MultirotorMixer&); - MultirotorMixer operator=(const MultirotorMixer&); + MultirotorMixer(const MultirotorMixer &); + MultirotorMixer operator=(const MultirotorMixer &); }; #endif diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index 9401655e21fa..cee01c83bddc 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -52,6 +52,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* open the mixer definition file */ fp = fopen(fname, "r"); + if (fp == NULL) { warnx("file not found"); return -1; @@ -59,29 +60,38 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* read valid lines from the file into a buffer */ buf[0] = '\0'; + for (;;) { /* get a line, bail on error/EOF */ line[0] = '\0'; - if (fgets(line, sizeof(line), fp) == NULL) + + if (fgets(line, sizeof(line), fp) == NULL) { break; + } /* if the line doesn't look like a mixer definition line, skip it */ - if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) { continue; + } /* compact whitespace in the buffer */ char *t, *f; + for (f = line; *f != '\0'; f++) { /* scan for space characters */ if (*f == ' ') { /* look for additional spaces */ t = f + 1; - while (*t == ' ') + + while (*t == ' ') { t++; + } + if (*t == '\0') { /* strip trailing whitespace */ *f = '\0'; + } else if (t > (f + 1)) { memmove(f + 1, t, strlen(t) + 1); } diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 369e6807d12a..38074f7ae5e9 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -109,15 +109,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl /* enforce that the mixer ends with space or a new line */ for (int i = buflen - 1; i >= 0; i--) { - if (buf[i] == '\0') + if (buf[i] == '\0') { continue; + } /* require a space or newline at the end of the buffer, fail on printable chars */ if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { /* found a line ending or space, so no split symbols / numbers. good. */ break; + } else { - debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]); + debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen - 1, buf[i]); return nullptr; } @@ -134,6 +136,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); return nullptr; @@ -170,7 +173,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "8x")) { geometry = MultirotorGeometry::OCTA_X; - + } else if (!strcmp(geomname, "8c")) { geometry = MultirotorGeometry::OCTA_COX; @@ -222,6 +225,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) if (status_reg != NULL) { (*status_reg) = 0; } + // thrust boost parameters float thrust_increase_factor = 1.5f; float thrust_decrease_factor = 0.6f; @@ -238,6 +242,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) if (out < min_out) { min_out = out; } + if (out > max_out) { max_out = out; } @@ -248,84 +253,105 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) float boost = 0.0f; // value added to demanded thrust (can also be negative) float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch - if(min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { + if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; - if(max_thrust_diff >= -min_out) { + + if (max_thrust_diff >= -min_out) { boost = -min_out; - } - else { + + } else { boost = max_thrust_diff; - roll_pitch_scale = (thrust + boost)/(thrust - min_out); + roll_pitch_scale = (thrust + boost) / (thrust - min_out); } - } - else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { - float max_thrust_diff = thrust - thrust_decrease_factor*thrust; - if(max_thrust_diff >= max_out - 1.0f) { + + } else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { + float max_thrust_diff = thrust - thrust_decrease_factor * thrust; + + if (max_thrust_diff >= max_out - 1.0f) { boost = -(max_out - 1.0f); + } else { boost = -max_thrust_diff; - roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust); + roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); } - } - else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { + + } else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; - boost = constrain(-min_out - (1.0f - max_out)/2.0f,0.0f, max_thrust_diff); - roll_pitch_scale = (thrust + boost)/(thrust - min_out); - } - else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f ) { - float max_thrust_diff = thrust - thrust_decrease_factor*thrust; - boost = constrain(-(max_out - 1.0f - min_out)/2.0f, -max_thrust_diff, 0.0f); - roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust); - } - else if (min_out < 0.0f && max_out > 1.0f) { - boost = constrain(-(max_out - 1.0f + min_out)/2.0f, thrust_decrease_factor*thrust - thrust, thrust_increase_factor*thrust - thrust); - roll_pitch_scale = (thrust + boost)/(thrust - min_out); + boost = constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff); + roll_pitch_scale = (thrust + boost) / (thrust - min_out); + + } else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) { + float max_thrust_diff = thrust - thrust_decrease_factor * thrust; + boost = constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f); + roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); + + } else if (min_out < 0.0f && max_out > 1.0f) { + boost = constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust, + thrust_increase_factor * thrust - thrust); + roll_pitch_scale = (thrust + boost) / (thrust - min_out); } // notify if saturation has occurred - if(min_out < 0.0f) { - if(status_reg != NULL) { + if (min_out < 0.0f) { + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT; } } - if(max_out > 0.0f) { - if(status_reg != NULL) { + + if (max_out > 0.0f) { + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT; } } // mix again but now with thrust boost, scale roll/pitch and also add yaw - for(unsigned i = 0; i < _rotor_count; i++) { + for (unsigned i = 0; i < _rotor_count; i++) { float out = (roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale) * roll_pitch_scale + - yaw * _rotors[i].yaw_scale + + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + + yaw * _rotors[i].yaw_scale + thrust + boost; out *= _rotors[i].out_scale; // scale yaw if it violates limits. inform about yaw limit reached - if(out < 0.0f) { - yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * - roll_pitch_scale + thrust + boost)/_rotors[i].yaw_scale; - if(status_reg != NULL) { + if (out < 0.0f) { + if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { + yaw = 0.0f; + + } else { + yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * + roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale; + } + + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; } - } - else if(out > 1.0f) { - yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * - roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale; - if(status_reg != NULL) { + + } else if (out > 1.0f) { + // allow to reduce thrust to get some yaw response + float thrust_reduction = fminf(0.15f, out - 1.0f); + thrust -= thrust_reduction; + + if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { + yaw = 0.0f; + + } else { + yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * + roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale; + } + + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; } } } - /* last mix, add yaw and scale outputs to range idle_speed...1 */ + /* add yaw and scale outputs to range idle_speed...1 */ for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = (roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale) * roll_pitch_scale + - yaw * _rotors[i].yaw_scale + - thrust + boost; + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust + boost; outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f); } diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index 693f23ede2fd..4ad20c8aee87 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -67,8 +67,9 @@ SimpleMixer::SimpleMixer(ControlCallback control_cb, SimpleMixer::~SimpleMixer() { - if (_info != nullptr) + if (_info != nullptr) { free(_info); + } } int @@ -77,8 +78,9 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler int ret; int s[5]; int n = -1; - + buf = findtag(buf, buflen, 'O'); + if ((buf == nullptr) || (buflen < 12)) { debug("output parser failed finding tag, ret: '%s'", buf); return -1; @@ -91,6 +93,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); return -1; @@ -106,12 +109,14 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler } int -SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index) +SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, + uint8_t &control_index) { unsigned u[2]; int s[5]; buf = findtag(buf, buflen, 'S'); + if ((buf == nullptr) || (buflen < 16)) { debug("control parser failed finding tag, ret: '%s'", buf); return -1; @@ -124,6 +129,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); return -1; @@ -156,6 +162,7 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); goto out; @@ -198,14 +205,16 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c out: - if (mixinfo != nullptr) + if (mixinfo != nullptr) { free(mixinfo); + } return sm; } SimpleMixer * -SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max) +SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, + uint16_t mid, uint16_t max) { SimpleMixer *sm = nullptr; mixer_simple_s *mixinfo = nullptr; @@ -258,8 +267,9 @@ SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, u out: - if (mixinfo != nullptr) + if (mixinfo != nullptr) { free(mixinfo); + } return sm; } @@ -269,11 +279,13 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { float sum = 0.0f; - if (_info == nullptr) + if (_info == nullptr) { return 0; + } - if (space < 1) + if (space < 1) { return 0; + } for (unsigned i = 0; i < _info->control_count; i++) { float input; @@ -293,8 +305,9 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) void SimpleMixer::groups_required(uint32_t &groups) { - for (unsigned i = 0; i < _info->control_count; i++) + for (unsigned i = 0; i < _info->control_count; i++) { groups |= 1 << _info->controls[i].control_group; + } } int @@ -305,14 +318,16 @@ SimpleMixer::check() /* sanity that presumes that a mixer includes a control no more than once */ /* max of 32 groups due to groups_required API */ - if (_info->control_count > 32) + if (_info->control_count > 32) { return -2; + } /* validate the output scaler */ ret = scale_check(_info->output_scaler); - if (ret != 0) + if (ret != 0) { return ret; + } /* validate input scalers */ for (unsigned i = 0; i < _info->control_count; i++) { @@ -328,8 +343,9 @@ SimpleMixer::check() /* validate the scaler */ ret = scale_check(_info->controls[i].scaler); - if (ret != 0) + if (ret != 0) { return (10 * i + ret); + } } return 0; diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index a866c843d7ca..89329d0b55ca 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -76,29 +76,43 @@ int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature) int errors = 0; // descriptor - if (F_write_byte(ADDR_OTP_START, 'P')) + if (F_write_byte(ADDR_OTP_START, 'P')) { errors++; - // write the 'P' from PX4. to first byte in OTP - if (F_write_byte(ADDR_OTP_START + 1, 'X')) - errors++; // write the 'P' from PX4. to first byte in OTP - if (F_write_byte(ADDR_OTP_START + 2, '4')) + } + + // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 1, 'X')) { + errors++; // write the 'P' from PX4. to first byte in OTP + } + + if (F_write_byte(ADDR_OTP_START + 2, '4')) { errors++; - if (F_write_byte(ADDR_OTP_START + 3, '\0')) + } + + if (F_write_byte(ADDR_OTP_START + 3, '\0')) { errors++; + } + //id_type - if (F_write_byte(ADDR_OTP_START + 4, id_type)) + if (F_write_byte(ADDR_OTP_START + 4, id_type)) { errors++; + } + // vid and pid are 4 bytes each - if (F_write_word(ADDR_OTP_START + 5, vid)) + if (F_write_word(ADDR_OTP_START + 5, vid)) { errors++; - if (F_write_word(ADDR_OTP_START + 9, pid)) + } + + if (F_write_word(ADDR_OTP_START + 9, pid)) { errors++; + } // leave some 19 bytes of space, and go to the next block... // then the auth sig starts for (int i = 0 ; i < 128 ; i++) { - if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i])) + if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i])) { errors++; + } } return errors; @@ -123,8 +137,9 @@ int lock_otp(void) // or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes. for (int i = 0 ; i < locksize ; i++) { - if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED)) + if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED)) { errors++; + } } return errors; diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index b0a0b01d035e..a9d9db974813 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -46,7 +46,10 @@ #include "perf_counter.h" #ifdef __PX4_QURT -#define dprintf(...) +#define dprintf(...) +#define ddeclare(...) +#else +#define ddeclare(...) __VA_ARGS__ #endif /** @@ -144,11 +147,13 @@ perf_alloc_once(enum perf_counter_type type, const char *name) if (type == handle->type) { /* they are the same counter */ return handle; + } else { /* same name but different type, assuming this is an error and not intended */ return NULL; } } + handle = (perf_counter_t)sq_next(&handle->link); } @@ -159,8 +164,9 @@ perf_alloc_once(enum perf_counter_type type, const char *name) void perf_free(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } sq_rem(&handle->link, &perf_counters); free(handle); @@ -169,8 +175,9 @@ perf_free(perf_counter_t handle) void perf_count(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_COUNT: @@ -178,38 +185,46 @@ perf_count(perf_counter_t handle) break; case PC_INTERVAL: { - struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - hrt_abstime now = hrt_absolute_time(); + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + hrt_abstime now = hrt_absolute_time(); - switch (pci->event_count) { - case 0: - pci->time_first = now; - break; - case 1: - pci->time_least = now - pci->time_last; - pci->time_most = now - pci->time_last; - pci->mean = pci->time_least / 1e6f; - pci->M2 = 0; - break; - default: { - hrt_abstime interval = now - pci->time_last; - if (interval < pci->time_least) - pci->time_least = interval; - if (interval > pci->time_most) - pci->time_most = interval; - // maintain mean and variance of interval in seconds - // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) - float dt = interval / 1e6f; - float delta_intvl = dt - pci->mean; - pci->mean += delta_intvl / pci->event_count; - pci->M2 += delta_intvl * (dt - pci->mean); + switch (pci->event_count) { + case 0: + pci->time_first = now; break; + + case 1: + pci->time_least = now - pci->time_last; + pci->time_most = now - pci->time_last; + pci->mean = pci->time_least / 1e6f; + pci->M2 = 0; + break; + + default: { + hrt_abstime interval = now - pci->time_last; + + if (interval < pci->time_least) { + pci->time_least = interval; + } + + if (interval > pci->time_most) { + pci->time_most = interval; + } + + // maintain mean and variance of interval in seconds + // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) + float dt = interval / 1e6f; + float delta_intvl = dt - pci->mean; + pci->mean += delta_intvl / pci->event_count; + pci->M2 += delta_intvl * (dt - pci->mean); + break; + } } + + pci->time_last = now; + pci->event_count++; + break; } - pci->time_last = now; - pci->event_count++; - break; - } default: break; @@ -219,8 +234,9 @@ perf_count(perf_counter_t handle) void perf_begin(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_ELAPSED: @@ -235,8 +251,9 @@ perf_begin(perf_counter_t handle) void perf_end(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_ELAPSED: { @@ -247,16 +264,19 @@ perf_end(perf_counter_t handle) if (elapsed < 0) { pce->event_overruns++; + } else { pce->event_count++; pce->time_total += elapsed; - if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) + if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) { pce->time_least = elapsed; + } - if (pce->time_most < (uint64_t)elapsed) + if (pce->time_most < (uint64_t)elapsed) { pce->time_most = elapsed; + } // maintain mean and variance of the elapsed time in seconds // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) @@ -291,16 +311,19 @@ perf_set(perf_counter_t handle, int64_t elapsed) if (elapsed < 0) { pce->event_overruns++; + } else { pce->event_count++; pce->time_total += elapsed; - if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) + if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) { pce->time_least = elapsed; + } - if (pce->time_most < (uint64_t)elapsed) + if (pce->time_most < (uint64_t)elapsed) { pce->time_most = elapsed; + } // maintain mean and variance of the elapsed time in seconds // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) @@ -322,8 +345,9 @@ perf_set(perf_counter_t handle, int64_t elapsed) void perf_cancel(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_ELAPSED: { @@ -343,8 +367,9 @@ perf_cancel(perf_counter_t handle) void perf_reset(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_COUNT: @@ -352,25 +377,25 @@ perf_reset(perf_counter_t handle) break; case PC_ELAPSED: { - struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - pce->event_count = 0; - pce->time_start = 0; - pce->time_total = 0; - pce->time_least = 0; - pce->time_most = 0; - break; - } + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + pce->event_count = 0; + pce->time_start = 0; + pce->time_total = 0; + pce->time_least = 0; + pce->time_most = 0; + break; + } case PC_INTERVAL: { - struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - pci->event_count = 0; - pci->time_event = 0; - pci->time_first = 0; - pci->time_last = 0; - pci->time_least = 0; - pci->time_most = 0; - break; - } + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + pci->event_count = 0; + pci->time_event = 0; + pci->time_first = 0; + pci->time_last = 0; + pci->time_least = 0; + pci->time_most = 0; + break; + } } } @@ -383,45 +408,45 @@ perf_print_counter(perf_counter_t handle) void perf_print_counter_fd(int fd, perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_COUNT: dprintf(fd, "%s: %llu events\n", - handle->name, - (unsigned long long)((struct perf_ctr_count *)handle)->event_count); + handle->name, + (unsigned long long)((struct perf_ctr_count *)handle)->event_count); break; case PC_ELAPSED: { - struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - float rms = sqrtf(pce->M2 / (pce->event_count-1)); - - dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n", - handle->name, - (unsigned long long)pce->event_count, - (unsigned long long)pce->event_overruns, - (unsigned long long)pce->time_total, - (unsigned long long)pce->time_total / pce->event_count, - (unsigned long long)pce->time_least, - (unsigned long long)pce->time_most, - (double)(1e6f * rms)); - break; - } + ddeclare(struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;) + ddeclare(float rms = sqrtf(pce->M2 / (pce->event_count - 1));) + dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n", + handle->name, + (unsigned long long)pce->event_count, + (unsigned long long)pce->event_overruns, + (unsigned long long)pce->time_total, + pce->event_count == 0 ? 0 : (unsigned long long)pce->time_total / pce->event_count, + (unsigned long long)pce->time_least, + (unsigned long long)pce->time_most, + (double)(1e6f * rms)); + break; + } case PC_INTERVAL: { - struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - float rms = sqrtf(pci->M2 / (pci->event_count-1)); - - dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n", - handle->name, - (unsigned long long)pci->event_count, - (unsigned long long)(pci->time_last - pci->time_first) / pci->event_count, - (unsigned long long)pci->time_least, - (unsigned long long)pci->time_most, - (double)(1e6f * rms)); - break; - } + ddeclare(struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;) + ddeclare(float rms = sqrtf(pci->M2 / (pci->event_count - 1));) + + dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n", + handle->name, + (unsigned long long)pci->event_count, + (unsigned long long)(pci->time_last - pci->time_first) / pci->event_count, + (unsigned long long)pci->time_least, + (unsigned long long)pci->time_most, + (double)(1e6f * rms)); + break; + } default: break; @@ -431,26 +456,28 @@ perf_print_counter_fd(int fd, perf_counter_t handle) uint64_t perf_event_count(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return 0; + } switch (handle->type) { case PC_COUNT: return ((struct perf_ctr_count *)handle)->event_count; case PC_ELAPSED: { - struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - return pce->event_count; - } + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + return pce->event_count; + } case PC_INTERVAL: { - struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - return pci->event_count; - } + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + return pci->event_count; + } default: break; } + return 0; } @@ -473,11 +500,13 @@ void perf_print_latency(int fd) { dprintf(fd, "bucket : events\n"); + for (int i = 0; i < latency_bucket_count; i++) { printf(" %4i : %li\n", latency_buckets[i], (long int)latency_counters[i]); } + // print the overflow bucket value - dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]); + dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count - 1], latency_counters[latency_bucket_count]); } void @@ -489,6 +518,7 @@ perf_reset_all(void) perf_reset(handle); handle = (perf_counter_t)sq_next(&handle->link); } + for (int i = 0; i <= latency_bucket_count; i++) { latency_counters[i] = 0; } diff --git a/src/modules/systemlib/ppm_decode.c b/src/modules/systemlib/ppm_decode.c index fe4ddde19088..06845a043b39 100644 --- a/src/modules/systemlib/ppm_decode.c +++ b/src/modules/systemlib/ppm_decode.c @@ -127,14 +127,16 @@ ppm_input_decode(bool reset, unsigned count) unsigned i; /* if we missed an edge, we have to give up */ - if (reset) + if (reset) { goto error; + } /* how long since the last edge? */ width = count - ppm.last_edge; - if (count < ppm.last_edge) - width += ppm.count_max; /* handle wrapped count */ + if (count < ppm.last_edge) { + width += ppm.count_max; /* handle wrapped count */ + } ppm.last_edge = count; @@ -175,8 +177,9 @@ ppm_input_decode(bool reset, unsigned count) } else { /* frame channel count matches expected, let's use it */ if (ppm.next_channel > PPM_MIN_CHANNELS) { - for (i = 0; i < ppm.next_channel; i++) + for (i = 0; i < ppm.next_channel; i++) { ppm_buffer[i] = ppm_temp_buffer[i]; + } ppm_last_valid_decode = hrt_absolute_time(); } @@ -199,8 +202,9 @@ ppm_input_decode(bool reset, unsigned count) case ARM: /* we expect a pulse giving us the first mark */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ + if (width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too long */ + } /* record the mark timing, expect an inactive edge */ ppm.last_mark = count; @@ -218,20 +222,23 @@ ppm_input_decode(bool reset, unsigned count) case ACTIVE: /* we expect a well-formed pulse */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ + if (width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too long */ + } /* determine the interval from the last mark */ interval = count - ppm.last_mark; ppm.last_mark = count; /* if the mark-mark timing is out of bounds, abandon the frame */ - if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) { goto error; + } /* if we have room to store the value, do so */ - if (ppm.next_channel < PPM_MAX_CHANNELS) + if (ppm.next_channel < PPM_MAX_CHANNELS) { ppm_temp_buffer[ppm.next_channel++] = interval; + } ppm.phase = INACTIVE; return; diff --git a/src/modules/systemlib/print_load_posix.c b/src/modules/systemlib/print_load_posix.c new file mode 100644 index 000000000000..5e12dc3323bc --- /dev/null +++ b/src/modules/systemlib/print_load_posix.c @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file print_load_posix.c + * + * Print the current system load. + * + * @author Lorenz Meier + */ + +#include +#include + +#include +#include +#include + +extern struct system_load_s system_load; + +#define CL "\033[K" // clear line + +void init_print_load_s(uint64_t t, struct print_load_s *s) +{ + + s->total_user_time = 0; + + s->running_count = 0; + s->blocked_count = 0; + + s->new_time = t; + s->interval_start_time = t; + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + s->last_times[i] = 0; + } + + s->interval_time_ms_inv = 0.f; +} + +void print_load(uint64_t t, int fd, struct print_load_s *print_state) +{ +} + diff --git a/src/modules/systemlib/printload.c b/src/modules/systemlib/printload.c new file mode 100644 index 000000000000..a179ddc51b5e --- /dev/null +++ b/src/modules/systemlib/printload.c @@ -0,0 +1,282 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file printload.c + * + * Print the current system load. + * + * @author Lorenz Meier + */ + +#include +#include + +#include +#include +#include + +extern struct system_load_s system_load; + +#define CL "\033[K" // clear line + +void init_print_load_s(uint64_t t, struct print_load_s *s) +{ + + s->total_user_time = 0; + + s->running_count = 0; + s->blocked_count = 0; + + s->new_time = t; + s->interval_start_time = t; + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + s->last_times[i] = 0; + } + + s->interval_time_ms_inv = 0.f; +} + +static const char * +tstate_name(const tstate_t s) +{ + switch (s) { + case TSTATE_TASK_INVALID: + return "init"; + + case TSTATE_TASK_PENDING: + return "PEND"; + + case TSTATE_TASK_READYTORUN: + return "READY"; + + case TSTATE_TASK_RUNNING: + return "RUN"; + + case TSTATE_TASK_INACTIVE: + return "inact"; + + case TSTATE_WAIT_SEM: + return "w:sem"; +#ifndef CONFIG_DISABLE_SIGNALS + + case TSTATE_WAIT_SIG: + return "w:sig"; +#endif +#ifndef CONFIG_DISABLE_MQUEUE + + case TSTATE_WAIT_MQNOTEMPTY: + return "w:mqe"; + + case TSTATE_WAIT_MQNOTFULL: + return "w:mqf"; +#endif +#ifdef CONFIG_PAGING + + case TSTATE_WAIT_PAGEFILL: + return "w:pgf"; +#endif + + default: + return "ERROR"; + } +} + +void print_load(uint64_t t, int fd, struct print_load_s *print_state) +{ + print_state->new_time = t; + + int i; + uint64_t curr_time_us; + uint64_t idle_time_us; + char *clear_line = ""; + + /* print system information */ + if (fd == 1) { + dprintf(fd, "\033[H"); /* move cursor home and clear screen */ + clear_line = CL; + } + + curr_time_us = t; + idle_time_us = system_load.tasks[0].total_runtime; + + if (print_state->new_time > print_state->interval_start_time) { + print_state->interval_time_ms_inv = 1.f / ((float)((print_state->new_time - print_state->interval_start_time) / 1000)); + } + + print_state->running_count = 0; + print_state->blocked_count = 0; + print_state->total_user_time = 0; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + uint64_t interval_runtime; + + if (system_load.tasks[i].valid) { + switch (system_load.tasks[i].tcb->task_state) { + case TSTATE_TASK_PENDING: + case TSTATE_TASK_READYTORUN: + case TSTATE_TASK_RUNNING: + print_state->running_count++; + break; + + case TSTATE_TASK_INVALID: + case TSTATE_TASK_INACTIVE: + case TSTATE_WAIT_SEM: +#ifndef CONFIG_DISABLE_SIGNALS + case TSTATE_WAIT_SIG: +#endif +#ifndef CONFIG_DISABLE_MQUEUE + case TSTATE_WAIT_MQNOTEMPTY: + case TSTATE_WAIT_MQNOTFULL: +#endif +#ifdef CONFIG_PAGING + case TSTATE_WAIT_PAGEFILL: +#endif + print_state->blocked_count++; + break; + } + } + + interval_runtime = (system_load.tasks[i].valid && print_state->last_times[i] > 0 && + system_load.tasks[i].total_runtime > print_state->last_times[i]) + ? (system_load.tasks[i].total_runtime - print_state->last_times[i]) / 1000 + : 0; + + print_state->last_times[i] = system_load.tasks[i].total_runtime; + + if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) { + print_state->curr_loads[i] = interval_runtime * print_state->interval_time_ms_inv; + + if (i > 0) { + print_state->total_user_time += interval_runtime; + } + + } else { + print_state->curr_loads[i] = 0; + } + } + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) { + if (system_load.tasks[i].tcb->pid == 0) { + float idle; + float task_load; + float sched_load; + + idle = print_state->curr_loads[0]; + task_load = (float)(print_state->total_user_time) * print_state->interval_time_ms_inv; + + /* this can happen if one tasks total runtime was not computed + correctly by the scheduler instrumentation TODO */ + if (task_load > (1.f - idle)) { + task_load = (1.f - idle); + } + + sched_load = 1.f - idle - task_load; + + dprintf(fd, "%sProcesses: %d total, %d running, %d sleeping\n", + clear_line, + system_load.total_count, + print_state->running_count, + print_state->blocked_count); + dprintf(fd, "%sCPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n", + clear_line, + (double)(task_load * 100.f), + (double)(sched_load * 100.f), + (double)(idle * 100.f)); + dprintf(fd, "%sUptime: %.3fs total, %.3fs idle\n%s\n", + clear_line, + (double)curr_time_us / 1000000.d, + (double)idle_time_us / 1000000.d, + clear_line); + + /* header for task list */ + dprintf(fd, "%s%4s %*-s %8s %6s %11s %10s %-6s\n", + clear_line, + "PID", + CONFIG_TASK_NAME_SIZE, "COMMAND", + "CPU(ms)", + "CPU(%)", + "USED/STACK", + "PRIO(BASE)", +#if CONFIG_RR_INTERVAL > 0 + "TSLICE" +#else + "STATE" +#endif + ); + } + + unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + unsigned stack_free = 0; + uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; + + while (stack_free < stack_size) { + if (*stack_sweeper++ != 0xff) { + break; + } + + stack_free++; + } + + dprintf(fd, "%s%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", + clear_line, + system_load.tasks[i].tcb->pid, + CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, + (system_load.tasks[i].total_runtime / 1000), + (int)(print_state->curr_loads[i] * 100.0f), + (int)((print_state->curr_loads[i] * 100.0f - (int)(print_state->curr_loads[i] * 100.0f)) * 1000), + stack_size - stack_free, + stack_size, + system_load.tasks[i].tcb->sched_priority, +#if CONFIG_ARCH_BOARD_SIM + 0); +#else + system_load.tasks[i].tcb->base_priority); +#endif + +#if CONFIG_RR_INTERVAL > 0 + /* print scheduling info with RR time slice */ + dprintf(fd, " %6d\n", system_load.tasks[i].tcb->timeslice); +#else + // print task state instead + dprintf(fd, " %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state)); +#endif + } + } + + print_state->interval_start_time = print_state->new_time; +} + diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h new file mode 100644 index 000000000000..ee210f8903d0 --- /dev/null +++ b/src/modules/systemlib/printload.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file printload.h + * + * Print the current system load. + * + * @author Lorenz Meier + */ + +#pragma once + +__BEGIN_DECLS + +#include +#include + +#ifndef CONFIG_MAX_TASKS +#define CONFIG_MAX_TASKS 64 +#endif + +struct print_load_s { + uint64_t total_user_time; + + int running_count; + int blocked_count; + + uint64_t new_time; + uint64_t interval_start_time; + uint64_t last_times[CONFIG_MAX_TASKS]; + float curr_loads[CONFIG_MAX_TASKS]; + float interval_time_ms_inv; +}; + +__EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s); + +__EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state); + +__END_DECLS diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index adcfb703c0e4..e64b8d635d84 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: Julian Oes + * 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 @@ -35,9 +34,10 @@ /** * @file pwm_limit.c * - * Lib to limit PWM output + * Library for PWM output limiting * - * @author Julian Oes + * @author Julian Oes + * @author Lorenz Meier */ #include "pwm_limit.h" @@ -46,6 +46,8 @@ #include #include +#define PROGRESS_INT_SCALING 10000 + void pwm_limit_init(pwm_limit_t *limit) { limit->state = PWM_LIMIT_STATE_INIT; @@ -53,127 +55,173 @@ void pwm_limit_init(pwm_limit_t *limit) return; } -void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask, - const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, - const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) +void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, + const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, + const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { /* first evaluate state changes */ switch (limit->state) { - case PWM_LIMIT_STATE_INIT: - - if (armed) { - - /* set arming time for the first call */ - if (limit->time_armed == 0) { - limit->time_armed = hrt_absolute_time(); - } + case PWM_LIMIT_STATE_INIT: - if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { - limit->state = PWM_LIMIT_STATE_OFF; - } - } - break; - case PWM_LIMIT_STATE_OFF: - if (armed) { - limit->state = PWM_LIMIT_STATE_RAMP; + if (armed) { - /* reset arming time, used for ramp timing */ + /* set arming time for the first call */ + if (limit->time_armed == 0) { limit->time_armed = hrt_absolute_time(); } - break; - case PWM_LIMIT_STATE_RAMP: - if (!armed) { - limit->state = PWM_LIMIT_STATE_OFF; - } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { - limit->state = PWM_LIMIT_STATE_ON; - } - break; - case PWM_LIMIT_STATE_ON: - if (!armed) { + + if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { limit->state = PWM_LIMIT_STATE_OFF; } - break; - default: - break; + } + + break; + + case PWM_LIMIT_STATE_OFF: + if (armed) { + limit->state = PWM_LIMIT_STATE_RAMP; + + /* reset arming time, used for ramp timing */ + limit->time_armed = hrt_absolute_time(); + } + + break; + + case PWM_LIMIT_STATE_RAMP: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + + } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { + limit->state = PWM_LIMIT_STATE_ON; + } + + break; + + case PWM_LIMIT_STATE_ON: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + } + + break; + + default: + break; + } + + /* if the system is pre-armed, the limit state is temporarily on, + * as some outputs are valid and the non-valid outputs have been + * set to NaN. This is not stored in the state machine though, + * as the throttle channels need to go through the ramp at + * regular arming time. + */ + + unsigned local_limit_state = limit->state; + + if (pre_armed) { + local_limit_state = PWM_LIMIT_STATE_ON; } unsigned progress; /* then set effective_pwm based on state */ - switch (limit->state) { - case PWM_LIMIT_STATE_OFF: - case PWM_LIMIT_STATE_INIT: - for (unsigned i=0; itime_armed); - - progress = diff * 10000 / RAMP_TIME_US; - - for (unsigned i=0; i 0) { - - /* safeguard against overflows */ - unsigned disarmed = disarmed_pwm[i]; - if (disarmed > min_pwm[i]) { - disarmed = min_pwm[i]; - } - - unsigned disarmed_min_diff = min_pwm[i] - disarmed; - ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; - - } else { - - /* no disarmed pwm value set, choose min pwm */ - ramp_min_pwm = min_pwm[i]; - } + switch (local_limit_state) { + case PWM_LIMIT_STATE_OFF: + case PWM_LIMIT_STATE_INIT: + for (unsigned i = 0; i < num_channels; i++) { + effective_pwm[i] = disarmed_pwm[i]; + } - float control_value = output[i]; + break; - if (reverse_mask & (1 << i)) { - control_value = -1.0f * control_value; - } + case PWM_LIMIT_STATE_RAMP: { + hrt_abstime diff = hrt_elapsed_time(&limit->time_armed); - effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; + progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; - /* last line of defense against invalid inputs */ - if (effective_pwm[i] < ramp_min_pwm) { - effective_pwm[i] = ramp_min_pwm; - } else if (effective_pwm[i] > max_pwm[i]) { - effective_pwm[i] = max_pwm[i]; - } - } + if (progress > PROGRESS_INT_SCALING) { + progress = PROGRESS_INT_SCALING; } - break; - case PWM_LIMIT_STATE_ON: - for (unsigned i=0; i 0) { + + /* safeguard against overflows */ + unsigned disarmed = disarmed_pwm[i]; + + if (disarmed > min_pwm[i]) { + disarmed = min_pwm[i]; + } + + unsigned disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; + + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + if (reverse_mask & (1 << i)) { control_value = -1.0f * control_value; } - effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; + effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm) / 2 + (max_pwm[i] + ramp_min_pwm) / 2; /* last line of defense against invalid inputs */ - if (effective_pwm[i] < min_pwm[i]) { - effective_pwm[i] = min_pwm[i]; + if (effective_pwm[i] < ramp_min_pwm) { + effective_pwm[i] = ramp_min_pwm; + } else if (effective_pwm[i] > max_pwm[i]) { effective_pwm[i] = max_pwm[i]; } } - break; - default: - break; + } + break; + + case PWM_LIMIT_STATE_ON: + for (unsigned i = 0; i < num_channels; i++) { + + float control_value = output[i]; + + /* check for invalid / disabled channels */ + if (!isfinite(control_value)) { + effective_pwm[i] = disarmed_pwm[i]; + continue; + } + + if (reverse_mask & (1 << i)) { + control_value = -1.0f * control_value; + } + + effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2; + + /* last line of defense against invalid inputs */ + if (effective_pwm[i] < min_pwm[i]) { + effective_pwm[i] = min_pwm[i]; + + } else if (effective_pwm[i] > max_pwm[i]) { + effective_pwm[i] = max_pwm[i]; + } + } + + break; + + default: + break; } + return; } diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 6ad3d755994f..23adc9ee3ab7 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Julian Oes + * 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 @@ -33,11 +32,11 @@ ****************************************************************************/ /** - * @file pwm_limit.h + * @file pwm_limit.c * - * Lib to limit PWM output + * Library for PWM output limiting * - * @author Julian Oes + * @author Julian Oes */ #ifndef PWM_LIMIT_H_ @@ -72,7 +71,8 @@ typedef struct { __EXPORT void pwm_limit_init(pwm_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm, +__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, + const uint16_t reverse_mask, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 25dea7a9b03a..622a3a0603c4 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -52,7 +52,7 @@ #define RC_INPUT_MAP_UNMAPPED 0 -int rc_calibration_check(int mavlink_fd) +int rc_calibration_check(int mavlink_fd, bool report_fail) { char nbuf[20]; @@ -74,7 +74,8 @@ int rc_calibration_check(int mavlink_fd) param_t map_parm = param_find(rc_map_mandatory[j]); if (map_parm == PARAM_INVALID) { - mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; @@ -86,14 +87,16 @@ int rc_calibration_check(int mavlink_fd) param_get(map_parm, &mapping); if (mapping > RC_INPUT_MAX_CHANNELS) { - mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; } if (mapping == 0) { - mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; @@ -144,35 +147,44 @@ int rc_calibration_check(int mavlink_fd) /* assert min..center..max ordering */ if (param_min < RC_INPUT_LOWEST_MIN_US) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); + + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_max > RC_INPUT_HIGHEST_MAX_US) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); + + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim < param_min) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); + + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim > param_max) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); + + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } + /* give system time to flush error message in case there are more */ usleep(100000); } /* assert deadzone is sane */ if (param_dz > RC_INPUT_MAX_DEADZONE_US) { - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } + /* give system time to flush error message in case there are more */ usleep(100000); count++; @@ -187,8 +199,13 @@ int rc_calibration_check(int mavlink_fd) if (channels_failed) { sleep(2); - mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count, - (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", + total_fail_count, + (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + } + usleep(100000); } diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index 035f63bef4a4..239c8a6a133f 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -47,6 +47,6 @@ __BEGIN_DECLS * @return 0 / OK if RC calibration is ok, index + 1 of the first * channel that failed else (so 1 == first channel failed) */ -__EXPORT int rc_calibration_check(int mavlink_fd); +__EXPORT int rc_calibration_check(int mavlink_fd, bool report_fail); __END_DECLS diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index 100c5ac499ec..f28b9180c1a3 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -51,16 +51,17 @@ class StateTable }; StateTable(Tran const *table, unsigned nStates, unsigned nSignals) - : myTable(table), myNsignals(nSignals) {} + : myTable(table), myNsignals(nSignals) {} - #define NO_ACTION &StateTable::doNothing - #define ACTION(_target) StateTable::Action(_target) +#define NO_ACTION &StateTable::doNothing +#define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} - void dispatch(unsigned const sig) { + void dispatch(unsigned const sig) + { /* get transition using state table */ - Tran const *t = myTable + myState*myNsignals + sig; + Tran const *t = myTable + myState * myNsignals + sig; /* accept new state */ myState = t->nextState; @@ -68,7 +69,8 @@ class StateTable /* */ (this->*(t->action))(); } - void doNothing() { + void doNothing() + { return; } protected: diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 454e5b5aa6fd..0bf1a3ea9e71 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -37,13 +37,10 @@ * System wide parameters */ -#include -#include - /** * Auto-start script index. * - * Defines the auto-start script used to bootstrap the system. + * CHANGING THIS VALUE REQUIRES A RESTART. Defines the auto-start script used to bootstrap the system. * * @group System */ @@ -87,10 +84,10 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); /** * Companion computer interface * -* Configures the baud rate of the companion computer interface. -* Set to zero to disable, set to 921600 to enable. -* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for -* other baud rates. +* CHANGING THIS VALUE REQUIRES A RESTART. Configures the baud rate of the companion computer interface. +* Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!) +* 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1. +* 157600: enables OSD mode at 57600 baud, 8N1. * * @min 0 * @max 921600 diff --git a/src/modules/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c index 41707360e4df..6460b03c9d26 100644 --- a/src/modules/systemlib/up_cxxinitialize.c +++ b/src/modules/systemlib/up_cxxinitialize.c @@ -126,27 +126,25 @@ extern uint32_t _etext; __EXPORT void up_cxxinitialize(void); void up_cxxinitialize(void) { - initializer_t *initp; + initializer_t *initp; - cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n", - &_sinit, &_einit, &_stext, &_etext); + cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n", + &_sinit, &_einit, &_stext, &_etext); - /* Visit each entry in the initialzation table */ + /* Visit each entry in the initialzation table */ - for (initp = &_sinit; initp != &_einit; initp++) - { - initializer_t initializer = *initp; - cxxdbg("initp: %p initializer: %p\n", initp, initializer); + for (initp = &_sinit; initp != &_einit; initp++) { + initializer_t initializer = *initp; + cxxdbg("initp: %p initializer: %p\n", initp, initializer); - /* Make sure that the address is non-NULL and lies in the text region - * defined by the linker script. Some toolchains may put NULL values - * or counts in the initialization table - */ + /* Make sure that the address is non-NULL and lies in the text region + * defined by the linker script. Some toolchains may put NULL values + * or counts in the initialization table + */ - if ((void*)initializer > (void*)&_stext && (void*)initializer < (void*)&_etext) - { - cxxdbg("Calling %p\n", initializer); - initializer(); - } - } + if ((void *)initializer > (void *)&_stext && (void *)initializer < (void *)&_etext) { + cxxdbg("Calling %p\n", initializer); + initializer(); + } + } } diff --git a/src/modules/systemlib/uthash/uthash.h b/src/modules/systemlib/uthash/uthash.h index 9f83fc34f15b..a8642833f62d 100644 --- a/src/modules/systemlib/uthash/uthash.h +++ b/src/modules/systemlib/uthash/uthash.h @@ -61,6 +61,7 @@ do { typedef unsigned int uint32_t; typedef unsigned char uint8_t; #else +#define __STDC_FORMAT_MACROS #include /* uint32_t */ #endif From 307777bfcae086b0c5d3f0f04ab1f89ccbf88769 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:07:52 +1100 Subject: [PATCH 227/293] uORB: update from upstream --- src/modules/uORB/topics/mission.h | 87 ++++------------- src/modules/uORB/topics/mission_result.h | 55 ++++++----- .../uORB/topics/multirotor_motor_limits.h | 40 ++++---- .../uORB/topics/navigation_capabilities.h | 38 ++++---- src/modules/uORB/topics/rc_parameter_map.h | 56 +++++------ src/modules/uORB/topics/safety.h | 46 +++++---- src/modules/uORB/topics/satellite_info.h | 64 ++++++------- src/modules/uORB/topics/telemetry_status.h | 93 ++++++++----------- 8 files changed, 219 insertions(+), 260 deletions(-) diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 22a8f3ecb8e6..1a26398e87b3 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -31,82 +31,36 @@ * ****************************************************************************/ -/** - * @file mission.h - * Definition of a mission consisting of mission items. - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - */ +/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/mission.msg */ + -#ifndef TOPIC_MISSION_H_ -#define TOPIC_MISSION_H_ +#pragma once #include -#include -#include "../uORB.h" +#include -#define NUM_MISSIONS_SUPPORTED 256 -/* compatible to mavlink MAV_CMD */ -enum NAV_CMD { - NAV_CMD_IDLE=0, - NAV_CMD_WAYPOINT=16, - NAV_CMD_LOITER_UNLIMITED=17, - NAV_CMD_LOITER_TURN_COUNT=18, - NAV_CMD_LOITER_TIME_LIMIT=19, - NAV_CMD_RETURN_TO_LAUNCH=20, - NAV_CMD_LAND=21, - NAV_CMD_TAKEOFF=22, - NAV_CMD_ROI=80, - NAV_CMD_PATHPLANNING=81, - NAV_CMD_DO_JUMP=177 -}; +#ifndef __cplusplus -enum ORIGIN { - ORIGIN_MAVLINK = 0, - ORIGIN_ONBOARD -}; +#endif /** * @addtogroup topics * @{ */ -/** - * Global position setpoint in WGS84 coordinates. - * - * This is the position the MAV is heading towards. If it of type loiter, - * the MAV is circling around it with the given loiter radius in meters. - */ -struct mission_item_s { - bool altitude_is_relative; /**< true if altitude is relative from start point */ - double lat; /**< latitude in degrees */ - double lon; /**< longitude in degrees */ - float altitude; /**< altitude in meters (AMSL) */ - float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ - float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ - int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ - enum NAV_CMD nav_cmd; /**< navigation command */ - float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ - float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ - float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ - bool autocontinue; /**< true if next waypoint should follow after this one */ - enum ORIGIN origin; /**< where the waypoint has been generated */ - int do_jump_mission_index; /**< index where the do jump will go to */ - unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */ - unsigned do_jump_current_count; /**< count how many times the jump has been done */ -}; -/** - * This topic used to notify navigator about mission changes, mission itself and new mission state - * must be stored in dataman before publication. - */ -struct mission_s -{ - int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ - unsigned count; /**< count of the missions stored in the dataman */ - int current_seq; /**< default -1, start at the one changed latest */ +#ifdef __cplusplus +struct __EXPORT mission_s { +#else +struct mission_s { +#endif + int32_t dataman_id; + uint32_t count; + int32_t current_seq; +#ifdef __cplusplus + +#endif }; /** @@ -114,7 +68,4 @@ struct mission_s */ /* register this as object request broker structure */ -ORB_DECLARE(offboard_mission); -ORB_DECLARE(onboard_mission); - -#endif +ORB_DECLARE(mission); diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 16e7f2f12672..9379b57add0d 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,38 +31,45 @@ * ****************************************************************************/ -/** - * @file mission_result.h - * Mission results that navigator needs to pass on to commander and mavlink. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * @author Ban Siesta - */ +/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/mission_result.msg */ + -#ifndef TOPIC_MISSION_RESULT_H -#define TOPIC_MISSION_RESULT_H +#pragma once #include -#include -#include "../uORB.h" +#include + + +#ifndef __cplusplus + +#endif /** * @addtogroup topics * @{ */ + +#ifdef __cplusplus +struct __EXPORT mission_result_s { +#else struct mission_result_s { - unsigned seq_reached; /**< Sequence of the mission item which has been reached */ - unsigned seq_current; /**< Sequence of the current mission item */ - bool reached; /**< true if mission has been reached */ - bool finished; /**< true if mission has been completed */ - bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ - bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ - bool item_do_jump_changed; /**< true if the number of do jumps remaining has changed */ - unsigned item_changed_index; /**< indicate which item has changed */ - unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */ +#endif + uint32_t instance_count; + uint32_t seq_reached; + uint32_t seq_current; + bool valid; + bool warning; + bool reached; + bool finished; + bool stay_in_failsafe; + bool flight_termination; + bool item_do_jump_changed; + uint32_t item_changed_index; + uint32_t item_do_jump_remaining; +#ifdef __cplusplus + +#endif }; /** @@ -71,5 +78,3 @@ struct mission_result_s { /* register this as object request broker structure */ ORB_DECLARE(mission_result); - -#endif diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h index 589f8a650cce..51a51a3c0dc9 100644 --- a/src/modules/uORB/topics/multirotor_motor_limits.h +++ b/src/modules/uORB/topics/multirotor_motor_limits.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-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 @@ -31,31 +31,37 @@ * ****************************************************************************/ -/** - * @file multirotor_motor_limits.h - * - * Definition of multirotor_motor_limits topic - */ +/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/multirotor_motor_limits.msg */ + -#ifndef MULTIROTOR_MOTOR_LIMITS_H_ -#define MULTIROTOR_MOTOR_LIMITS_H_ +#pragma once -#include "../uORB.h" #include +#include + + +#ifndef __cplusplus + +#endif /** * @addtogroup topics * @{ */ -/** - * Motor limits - */ + +#ifdef __cplusplus +struct __EXPORT multirotor_motor_limits_s { +#else struct multirotor_motor_limits_s { - uint8_t lower_limit : 1; // at least one actuator command has saturated on the lower limit - uint8_t upper_limit : 1; // at least one actuator command has saturated on the upper limit - uint8_t yaw : 1; // yaw limit reached - uint8_t reserved : 5; // reserved +#endif + uint8_t lower_limit; + uint8_t upper_limit; + uint8_t yaw; + uint8_t reserved; +#ifdef __cplusplus + +#endif }; /** @@ -64,5 +70,3 @@ struct multirotor_motor_limits_s { /* register this as object request broker structure */ ORB_DECLARE(multirotor_motor_limits); - -#endif diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h index 7a5ae9891484..67503173ec02 100644 --- a/src/modules/uORB/topics/navigation_capabilities.h +++ b/src/modules/uORB/topics/navigation_capabilities.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-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 @@ -31,33 +31,39 @@ * ****************************************************************************/ -/** - * @file navigation_capabilities.h - * - * Definition of navigation capabilities uORB topic. - */ +/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/navigation_capabilities.msg */ -#ifndef TOPIC_NAVIGATION_CAPABILITIES_H_ -#define TOPIC_NAVIGATION_CAPABILITIES_H_ -#include "../uORB.h" +#pragma once + #include +#include + + +#ifndef __cplusplus + +#endif /** * @addtogroup topics * @{ */ -/** - * Airspeed - */ -struct navigation_capabilities_s { - float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ - /* Landing parameters: see fw_pos_control_l1/landingslope.h */ +#ifdef __cplusplus +struct __EXPORT navigation_capabilities_s { +#else +struct navigation_capabilities_s { +#endif + uint64_t timestamp; + float turn_distance; float landing_horizontal_slope_displacement; float landing_slope_angle_rad; float landing_flare_length; + bool abort_landing; +#ifdef __cplusplus + +#endif }; /** @@ -66,5 +72,3 @@ struct navigation_capabilities_s { /* register this as object request broker structure */ ORB_DECLARE(navigation_capabilities); - -#endif diff --git a/src/modules/uORB/topics/rc_parameter_map.h b/src/modules/uORB/topics/rc_parameter_map.h index 6e68dc4b6f56..cc6e6761f67f 100644 --- a/src/modules/uORB/topics/rc_parameter_map.h +++ b/src/modules/uORB/topics/rc_parameter_map.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -20,7 +20,7 @@ * "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 , + * 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 @@ -31,46 +31,50 @@ * ****************************************************************************/ -/** - * @file rc_parameter_map.h - * Maps RC channels to parameters - * - * @author Thomas Gubler - */ +/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/rc_parameter_map.msg */ + -#ifndef TOPIC_RC_PARAMETER_MAP_H -#define TOPIC_RC_PARAMETER_MAP_H +#pragma once #include -#include "../uORB.h" +#include -#define RC_PARAM_MAP_NCHAN 3 // This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h -#define PARAM_ID_LEN 16 // corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + +#ifndef __cplusplus +#define RC_PARAM_MAP_NCHAN 3 +#define PARAM_ID_LEN 16 + +#endif /** * @addtogroup topics * @{ */ -struct rc_parameter_map_s { - uint64_t timestamp; /**< time at which the map was updated */ - bool valid[RC_PARAM_MAP_NCHAN]; /**< true for RC-Param channels which are mapped to a param */ +#ifdef __cplusplus +struct __EXPORT rc_parameter_map_s { +#else +struct rc_parameter_map_s { +#endif + uint64_t timestamp; + bool valid[3]; + int32_t param_index[3]; + char param_id[51]; + float scale[3]; + float value0[3]; + float value_min[3]; + float value_max[3]; +#ifdef __cplusplus + static const uint8_t RC_PARAM_MAP_NCHAN = 3; + static const uint8_t PARAM_ID_LEN = 16; - int param_index[RC_PARAM_MAP_NCHAN]; /**< corresponding param index, this - this field is ignored if set to -1, in this case param_id will - be used*/ - char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1]; /**< corresponding param id, null terminated */ - float scale[RC_PARAM_MAP_NCHAN]; /** scale to map the RC input [-1, 1] to a parameter value */ - float value0[RC_PARAM_MAP_NCHAN]; /** inital value around which the parameter value is changed */ - float value_min[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ - float value_max[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ +#endif }; /** * @} */ +/* register this as object request broker structure */ ORB_DECLARE(rc_parameter_map); - -#endif diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h index a5d21cd4ad98..d1ca994ddfed 100644 --- a/src/modules/uORB/topics/safety.h +++ b/src/modules/uORB/topics/safety.h @@ -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 @@ -31,27 +31,41 @@ * ****************************************************************************/ -/** - * @file safety.h - * - * Safety topic to pass safety state from px4io driver to commander - * This concerns only the safety button of the px4io but has nothing to do - * with arming/disarming. - */ +/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/safety.msg */ + -#ifndef TOPIC_SAFETY_H -#define TOPIC_SAFETY_H +#pragma once #include -#include "../uORB.h" +#include + +#ifndef __cplusplus + +#endif + +/** + * @addtogroup topics + * @{ + */ + + +#ifdef __cplusplus +struct __EXPORT safety_s { +#else struct safety_s { +#endif + uint64_t timestamp; + bool safety_switch_available; + bool safety_off; +#ifdef __cplusplus - uint64_t timestamp; - bool safety_switch_available; /**< Set to true if a safety switch is connected */ - bool safety_off; /**< Set to true if safety is off */ +#endif }; -ORB_DECLARE(safety); +/** + * @} + */ -#endif \ No newline at end of file +/* register this as object request broker structure */ +ORB_DECLARE(safety); diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h index 37c2faa96209..f3363b466df4 100644 --- a/src/modules/uORB/topics/satellite_info.h +++ b/src/modules/uORB/topics/satellite_info.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier + * 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,50 +31,43 @@ * ****************************************************************************/ -/** - * @file satellite_info.h - * Definition of the GNSS satellite info uORB topic. - */ +/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/satellite_info.msg */ + -#ifndef TOPIC_SAT_INFO_H_ -#define TOPIC_SAT_INFO_H_ +#pragma once #include -#include "../uORB.h" +#include + + +#ifndef __cplusplus +#define SAT_INFO_MAX_SATELLITES 20 + +#endif /** * @addtogroup topics * @{ */ -/** - * GNSS Satellite Info. - */ - -#define SAT_INFO_MAX_SATELLITES 20 +#ifdef __cplusplus +struct __EXPORT satellite_info_s { +#else struct satellite_info_s { - uint64_t timestamp; /**< Timestamp of satellite info */ - uint8_t count; /**< Number of satellites in satellite info */ - uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ - uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ - uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ -}; +#endif + uint64_t timestamp; + uint8_t count; + uint8_t svid[20]; + uint8_t used[20]; + uint8_t elevation[20]; + uint8_t azimuth[20]; + uint8_t snr[20]; +#ifdef __cplusplus + static const uint8_t SAT_INFO_MAX_SATELLITES = 20; -/** - * NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs - * u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf - * - * GPS 1-32 - * SBAS 120-158 - * Galileo 211-246 - * BeiDou 159-163, 33-64 - * QZSS 193-197 - * GLONASS 65-96, 255 - * - */ +#endif +}; /** * @} @@ -85,5 +75,3 @@ struct satellite_info_s { /* register this as object request broker structure */ ORB_DECLARE(satellite_info); - -#endif diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index ee950f4db275..93c27368bd16 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -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 @@ -31,72 +31,61 @@ * ****************************************************************************/ -/** - * @file telemetry_status.h - * - * Telemetry status topics - radio status outputs - */ +/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/telemetry_status.msg */ + -#ifndef TOPIC_TELEMETRY_STATUS_H -#define TOPIC_TELEMETRY_STATUS_H +#pragma once #include -#include "../uORB.h" +#include -enum TELEMETRY_STATUS_RADIO_TYPE { - TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0, - TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO, - TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET, - TELEMETRY_STATUS_RADIO_TYPE_WIRE -}; + +#ifndef __cplusplus +#define TELEMETRY_STATUS_RADIO_TYPE_GENERIC 0 +#define TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO 1 +#define TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET 2 +#define TELEMETRY_STATUS_RADIO_TYPE_WIRE 3 +#define TELEMETRY_STATUS_RADIO_TYPE_USB 4 + +#endif /** * @addtogroup topics * @{ */ + +#ifdef __cplusplus +struct __EXPORT telemetry_status_s { +#else struct telemetry_status_s { +#endif uint64_t timestamp; - uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ - uint64_t telem_time; /**< Time of last received telemetry status packet, 0 for none */ - enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ - uint8_t rssi; /**< local signal strength */ - uint8_t remote_rssi; /**< remote signal strength */ - uint16_t rxerrors; /**< receive errors */ - uint16_t fixed; /**< count of error corrected packets */ - uint8_t noise; /**< background noise level */ - uint8_t remote_noise; /**< remote background noise level */ - uint8_t txbuf; /**< how full the tx buffer is as a percentage */ - uint8_t system_id; /**< system id of the remote system */ - uint8_t component_id; /**< component id of the remote system */ + uint64_t heartbeat_time; + uint64_t telem_time; + uint8_t type; + uint8_t rssi; + uint8_t remote_rssi; + uint16_t rxerrors; + uint16_t fixed; + uint8_t noise; + uint8_t remote_noise; + uint8_t txbuf; + uint8_t system_id; + uint8_t component_id; +#ifdef __cplusplus + static const uint8_t TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0; + static const uint8_t TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1; + static const uint8_t TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2; + static const uint8_t TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3; + static const uint8_t TELEMETRY_STATUS_RADIO_TYPE_USB = 4; + +#endif }; /** * @} */ -ORB_DECLARE(telemetry_status_0); -ORB_DECLARE(telemetry_status_1); -ORB_DECLARE(telemetry_status_2); -ORB_DECLARE(telemetry_status_3); - -#define TELEMETRY_STATUS_ORB_ID_NUM 4 - -static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_ID_NUM] = { - ORB_ID(telemetry_status_0), - ORB_ID(telemetry_status_1), - ORB_ID(telemetry_status_2), - ORB_ID(telemetry_status_3), -}; - -// This is a hack to quiet an unused-variable warning for when telemetry_status.h is -// included but telemetry_status_orb_id is not referenced. The inline works if you -// choose to use it, but you can continue to just directly index into the array as well. -// If you don't use the inline this ends up being a no-op with no additional code emitted. -//static const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index); -static inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index) -{ - return telemetry_status_orb_id[index]; -} - -#endif /* TOPIC_TELEMETRY_STATUS_H */ +/* register this as object request broker structure */ +ORB_DECLARE(telemetry_status); From aa72fdf22e86302a2e5d7c7a849b28e5e9e0af06 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:08:16 +1100 Subject: [PATCH 228/293] uavcan: update for new debug macros --- src/modules/uavcan/sensors/baro.cpp | 4 ++-- src/modules/uavcan/sensors/mag.cpp | 2 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 10 +++++----- src/modules/uavcan/uavcan_main.cpp | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 9c3a0aefb4a1..92dc9b892490 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -61,7 +61,7 @@ int UavcanBarometerBridge::init() res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); if (res < 0) { - log("failed to start uavcan sub: %d", res); + DEVICE_LOG("failed to start uavcan sub: %d", res); return res; } return 0; @@ -95,7 +95,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) if ((arg < 80000) || (arg > 120000)) { return -EINVAL; } else { - log("new msl pressure %u", _msl_pressure); + DEVICE_LOG("new msl pressure %u", _msl_pressure); _msl_pressure = arg; return OK; } diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 48c4d20dff8c..6ef94090c9ff 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -61,7 +61,7 @@ int UavcanMagnetometerBridge::init() res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); if (res < 0) { - log("failed to start uavcan sub: %d", res); + DEVICE_LOG("failed to start uavcan sub: %d", res); return res; } return 0; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 75451ce84834..fa97b3c89e76 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -85,7 +85,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) return; // Give up immediately - saves some CPU time } - log("adding channel %d...", node_id); + DEVICE_LOG("adding channel %d...", node_id); // Search for the first free channel for (unsigned i = 0; i < _max_channels; i++) { @@ -98,7 +98,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) // No free channels left if (channel == nullptr) { _out_of_channels = true; - log("out of channels"); + DEVICE_LOG("out of channels"); return; } @@ -109,7 +109,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) const int class_instance = register_class_devname(_class_devname); if (class_instance < 0 || class_instance >= int(_max_channels)) { _out_of_channels = true; - log("out of class instances"); + DEVICE_LOG("out of class instances"); (void)unregister_class_devname(_class_devname, class_instance); return; } @@ -120,13 +120,13 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH); if (channel->orb_advert == nullptr) { - log("ADVERTISE FAILED"); + DEVICE_LOG("ADVERTISE FAILED"); (void)unregister_class_devname(_class_devname, class_instance); *channel = Channel(); return; } - log("channel %d class instance %d ok", channel->node_id, channel->class_instance); + DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance); } assert(channel != nullptr); diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index c11693cca392..28b71924eb97 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -384,7 +384,7 @@ int UavcanNode::run() // this would be bad... if (poll_ret < 0) { - log("poll error %d", errno); + DEVICE_LOG("poll error %d", errno); continue; } else { // get controls for required topics From fd743c1f333c9f2e394a2e93dfb4bc176b3cfbdd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:09:38 +1100 Subject: [PATCH 229/293] platforms: update from upstream --- src/platforms/common/CMakeLists.txt | 48 + src/platforms/common/px4_getopt.c | 48 +- src/platforms/nuttx/CMakeLists.txt | 42 + src/platforms/nuttx/px4_layer/CMakeLists.txt | 43 + src/platforms/nuttx/px4_layer/module.mk | 3 +- .../nuttx/px4_layer/px4_nuttx_tasks.c | 9 +- .../posix/drivers/accelsim/CMakeLists.txt | 42 + .../posix/drivers/accelsim/accelsim.cpp | 502 +++++----- .../posix/drivers/adcsim/CMakeLists.txt | 43 + src/platforms/posix/drivers/adcsim/adcsim.cpp | 25 +- .../posix/drivers/airspeedsim/CMakeLists.txt | 44 + .../posix/drivers/airspeedsim/airspeedsim.cpp | 138 ++- .../posix/drivers/airspeedsim/airspeedsim.h | 27 +- .../drivers/airspeedsim/meas_airspeed_sim.cpp | 625 +++++++++++++ .../posix/drivers/airspeedsim/module.mk | 3 +- .../posix/drivers/barosim/CMakeLists.txt | 44 + src/platforms/posix/drivers/barosim/baro.cpp | 228 ++--- .../posix/drivers/barosim/baro_sim.cpp | 35 +- src/platforms/posix/drivers/barosim/barosim.h | 2 +- .../posix/drivers/gpssim/CMakeLists.txt | 43 + src/platforms/posix/drivers/gpssim/gpssim.cpp | 605 ++++++++++++ src/platforms/posix/drivers/gpssim/module.mk | 42 + .../posix/drivers/gpssim/ubx_sim.cpp | 96 ++ src/platforms/posix/drivers/gpssim/ubx_sim.h | 57 ++ .../posix/drivers/gyrosim/CMakeLists.txt | 44 + .../posix/drivers/gyrosim/gyrosim.cpp | 870 ++++++------------ .../posix/drivers/tonealrmsim/CMakeLists.txt | 43 + .../posix/drivers/tonealrmsim/tone_alarm.cpp | 190 ++-- src/platforms/posix/include/board_config.h | 2 + src/platforms/posix/include/crc32.h | 2 +- src/platforms/posix/include/hrt_work.h | 63 ++ .../posix/include/px4_platform_types.h | 3 + src/platforms/posix/include/queue.h | 32 +- src/platforms/posix/main.cpp | 255 ++++- src/platforms/posix/px4_layer/CMakeLists.txt | 47 + src/platforms/posix/px4_layer/drv_hrt.c | 132 ++- src/platforms/posix/px4_layer/lib_crc32.c | 85 +- src/platforms/posix/px4_layer/module.mk | 13 +- src/platforms/posix/px4_layer/px4_log.c | 30 + .../posix/px4_layer/px4_posix_impl.cpp | 28 +- .../posix/px4_layer/px4_posix_tasks.cpp | 151 +-- src/platforms/posix/px4_layer/px4_sem.cpp | 111 +++ .../posix/tests/hello/CMakeLists.txt | 43 + .../posix/tests/hello/hello_example.cpp | 5 +- .../posix/tests/hello/hello_example.h | 3 +- .../posix/tests/hello/hello_start_posix.cpp | 12 +- .../posix/tests/hrt_test/CMakeLists.txt | 43 + .../posix/tests/hrt_test/hrt_test.cpp | 3 +- src/platforms/posix/tests/hrt_test/hrt_test.h | 3 +- .../tests/hrt_test/hrt_test_start_posix.cpp | 10 +- .../posix/tests/muorb/CMakeLists.txt | 43 + src/platforms/posix/tests/muorb/module.mk | 47 + .../posix/tests/muorb/muorb_test_example.cpp | 184 ++++ .../posix/tests/muorb/muorb_test_example.h | 62 ++ .../posix/tests/muorb/muorb_test_main.cpp | 59 ++ .../tests/muorb/muorb_test_start_posix.cpp | 101 ++ .../posix/tests/vcdev_test/CMakeLists.txt | 43 + .../tests/vcdev_test/vcdevtest_example.cpp | 279 ++++-- .../tests/vcdev_test/vcdevtest_example.h | 5 +- .../vcdev_test/vcdevtest_start_posix.cpp | 12 +- .../posix/tests/wqueue/CMakeLists.txt | 43 + .../posix/tests/wqueue/wqueue_start_posix.cpp | 12 +- .../posix/tests/wqueue/wqueue_test.cpp | 10 +- .../posix/tests/wqueue/wqueue_test.h | 9 +- src/platforms/posix/work_queue/CMakeLists.txt | 55 ++ src/platforms/posix/work_queue/dq_addlast.c | 73 ++ src/platforms/posix/work_queue/dq_rem.c | 81 ++ src/platforms/posix/work_queue/dq_remfirst.c | 81 ++ src/platforms/posix/work_queue/hrt_queue.c | 133 +++ src/platforms/posix/work_queue/hrt_thread.c | 284 ++++++ .../posix/work_queue/hrt_work_cancel.c | 111 +++ src/platforms/posix/work_queue/module.mk | 54 ++ src/platforms/posix/work_queue/queue.c | 98 ++ src/platforms/posix/work_queue/sq_addafter.c | 69 ++ src/platforms/posix/work_queue/sq_addlast.c | 72 ++ src/platforms/posix/work_queue/sq_remfirst.c | 76 ++ src/platforms/posix/work_queue/work_cancel.c | 120 +++ src/platforms/posix/work_queue/work_lock.c | 49 + src/platforms/posix/work_queue/work_lock.h | 43 + src/platforms/posix/work_queue/work_queue.c | 137 +++ src/platforms/posix/work_queue/work_thread.c | 327 +++++++ src/platforms/px4_adc.h | 11 +- src/platforms/px4_app.h | 10 +- src/platforms/px4_config.h | 2 +- src/platforms/px4_defines.h | 56 +- src/platforms/px4_i2c.h | 17 +- src/platforms/px4_includes.h | 4 + src/platforms/px4_log.h | 369 +++++++- src/platforms/px4_nodehandle.h | 17 +- src/platforms/px4_posix.h | 64 +- src/platforms/px4_publisher.h | 10 +- src/platforms/px4_spi.h | 39 +- src/platforms/px4_subscriber.h | 32 +- src/platforms/px4_tasks.h | 27 +- src/platforms/px4_time.h | 20 +- src/platforms/px4_workqueue.h | 29 +- src/platforms/qurt/dspal/dspal_stub.c | 76 ++ src/platforms/qurt/include/crc32.h | 2 +- src/platforms/qurt/include/hrt_work.h | 65 ++ src/platforms/qurt/include/i2c.h | 11 +- src/platforms/qurt/include/queue.h | 32 +- src/platforms/qurt/include/qurt_log.h | 64 ++ src/platforms/qurt/include/sched.h | 3 +- src/platforms/qurt/px4_layer/CMakeLists.txt | 73 ++ src/platforms/qurt/px4_layer/commands_hil.c | 108 +++ src/platforms/qurt/px4_layer/commands_muorb.c | 80 ++ .../qurt/px4_layer/commands_release.c | 175 ++++ src/platforms/qurt/px4_layer/commands_test.c | 68 ++ src/platforms/qurt/px4_layer/drv_hrt.c | 32 +- src/platforms/qurt/px4_layer/lib_crc32.c | 85 +- src/platforms/qurt/px4_layer/main.cpp | 125 ++- src/platforms/qurt/px4_layer/module.mk | 25 +- src/platforms/qurt/px4_layer/params.c | 43 + .../qurt/px4_layer/px4_qurt_impl.cpp | 44 +- .../qurt/px4_layer/px4_qurt_tasks.cpp | 167 +++- src/platforms/qurt/px4_layer/qurt_stubs.c | 120 +++ src/platforms/qurt/stubs/stubs_posix.c | 99 ++ src/platforms/qurt/stubs/stubs_qurt.c | 16 + src/platforms/qurt/tests/hello/CMakeLists.txt | 43 + .../qurt/tests/hello/hello_example.cpp | 5 +- .../qurt/tests/hello/hello_example.h | 3 +- .../qurt/tests/hello/hello_start_qurt.cpp | 10 +- src/platforms/qurt/tests/muorb/CMakeLists.txt | 42 + src/platforms/qurt/tests/muorb/module.mk | 48 + .../qurt/tests/muorb/muorb_test_example.cpp | 215 +++++ .../qurt/tests/muorb/muorb_test_example.h | 59 ++ .../qurt/tests/muorb/muorb_test_main.cpp | 57 ++ .../tests/muorb/muorb_test_start_qurt.cpp | 114 +++ src/platforms/ros/geo.cpp | 15 +- .../ros/nodes/commander/commander.cpp | 41 +- src/platforms/ros/nodes/commander/commander.h | 6 +- .../demo_offboard_attitude_setpoints.cpp | 7 +- .../ros/nodes/manual_input/manual_input.cpp | 5 +- src/platforms/ros/nodes/mavlink/mavlink.cpp | 103 ++- src/platforms/ros/nodes/mavlink/mavlink.h | 2 +- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 5 + .../position_estimator/position_estimator.cpp | 3 +- .../position_estimator/position_estimator.h | 2 +- 138 files changed, 8912 insertions(+), 1882 deletions(-) create mode 100644 src/platforms/common/CMakeLists.txt create mode 100644 src/platforms/nuttx/CMakeLists.txt create mode 100644 src/platforms/nuttx/px4_layer/CMakeLists.txt create mode 100644 src/platforms/posix/drivers/accelsim/CMakeLists.txt create mode 100644 src/platforms/posix/drivers/adcsim/CMakeLists.txt create mode 100644 src/platforms/posix/drivers/airspeedsim/CMakeLists.txt create mode 100644 src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp create mode 100644 src/platforms/posix/drivers/barosim/CMakeLists.txt create mode 100644 src/platforms/posix/drivers/gpssim/CMakeLists.txt create mode 100644 src/platforms/posix/drivers/gpssim/gpssim.cpp create mode 100644 src/platforms/posix/drivers/gpssim/module.mk create mode 100644 src/platforms/posix/drivers/gpssim/ubx_sim.cpp create mode 100644 src/platforms/posix/drivers/gpssim/ubx_sim.h create mode 100644 src/platforms/posix/drivers/gyrosim/CMakeLists.txt create mode 100644 src/platforms/posix/drivers/tonealrmsim/CMakeLists.txt create mode 100644 src/platforms/posix/include/hrt_work.h create mode 100644 src/platforms/posix/include/px4_platform_types.h create mode 100644 src/platforms/posix/px4_layer/CMakeLists.txt create mode 100644 src/platforms/posix/px4_layer/px4_log.c create mode 100644 src/platforms/posix/px4_layer/px4_sem.cpp create mode 100644 src/platforms/posix/tests/hello/CMakeLists.txt create mode 100644 src/platforms/posix/tests/hrt_test/CMakeLists.txt create mode 100644 src/platforms/posix/tests/muorb/CMakeLists.txt create mode 100644 src/platforms/posix/tests/muorb/module.mk create mode 100644 src/platforms/posix/tests/muorb/muorb_test_example.cpp create mode 100644 src/platforms/posix/tests/muorb/muorb_test_example.h create mode 100644 src/platforms/posix/tests/muorb/muorb_test_main.cpp create mode 100644 src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp create mode 100644 src/platforms/posix/tests/vcdev_test/CMakeLists.txt create mode 100644 src/platforms/posix/tests/wqueue/CMakeLists.txt create mode 100644 src/platforms/posix/work_queue/CMakeLists.txt create mode 100644 src/platforms/posix/work_queue/dq_addlast.c create mode 100644 src/platforms/posix/work_queue/dq_rem.c create mode 100644 src/platforms/posix/work_queue/dq_remfirst.c create mode 100644 src/platforms/posix/work_queue/hrt_queue.c create mode 100644 src/platforms/posix/work_queue/hrt_thread.c create mode 100644 src/platforms/posix/work_queue/hrt_work_cancel.c create mode 100644 src/platforms/posix/work_queue/module.mk create mode 100644 src/platforms/posix/work_queue/queue.c create mode 100644 src/platforms/posix/work_queue/sq_addafter.c create mode 100644 src/platforms/posix/work_queue/sq_addlast.c create mode 100644 src/platforms/posix/work_queue/sq_remfirst.c create mode 100644 src/platforms/posix/work_queue/work_cancel.c create mode 100644 src/platforms/posix/work_queue/work_lock.c create mode 100644 src/platforms/posix/work_queue/work_lock.h create mode 100644 src/platforms/posix/work_queue/work_queue.c create mode 100644 src/platforms/posix/work_queue/work_thread.c create mode 100644 src/platforms/qurt/dspal/dspal_stub.c create mode 100644 src/platforms/qurt/include/hrt_work.h create mode 100644 src/platforms/qurt/include/qurt_log.h create mode 100644 src/platforms/qurt/px4_layer/CMakeLists.txt create mode 100644 src/platforms/qurt/px4_layer/commands_hil.c create mode 100644 src/platforms/qurt/px4_layer/commands_muorb.c create mode 100644 src/platforms/qurt/px4_layer/commands_release.c create mode 100644 src/platforms/qurt/px4_layer/commands_test.c create mode 100644 src/platforms/qurt/px4_layer/params.c create mode 100644 src/platforms/qurt/px4_layer/qurt_stubs.c create mode 100644 src/platforms/qurt/stubs/stubs_posix.c create mode 100644 src/platforms/qurt/stubs/stubs_qurt.c create mode 100644 src/platforms/qurt/tests/hello/CMakeLists.txt create mode 100644 src/platforms/qurt/tests/muorb/CMakeLists.txt create mode 100644 src/platforms/qurt/tests/muorb/module.mk create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_example.cpp create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_example.h create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_main.cpp create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp diff --git a/src/platforms/common/CMakeLists.txt b/src/platforms/common/CMakeLists.txt new file mode 100644 index 000000000000..f7485a692c4c --- /dev/null +++ b/src/platforms/common/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +set(depends + msg_gen + prebuild_targets + git_mavlink + git_uavcan + git_matrix + ) + +px4_add_module( + MODULE platforms__common + SRCS + px4_getopt.c + DEPENDS + ${depends} + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/common/px4_getopt.c b/src/platforms/common/px4_getopt.c index d7d03ed49f77..863b5816b45b 100644 --- a/src/platforms/common/px4_getopt.c +++ b/src/platforms/common/px4_getopt.c @@ -44,13 +44,19 @@ static char isvalidopt(char p, const char *options, int *takesarg) { int idx = 0; *takesarg = 0; - while (options[idx] != 0 && p != options[idx]) + + while (options[idx] != 0 && p != options[idx]) { ++idx; - if (options[idx] == 0) + } + + if (options[idx] == 0) { return '?'; - if (options[idx+1] == ':') { + } + + if (options[idx + 1] == ':') { *takesarg = 1; } + return options[idx]; } @@ -67,40 +73,50 @@ static int reorder(int argc, char **argv, const char *options) while (idx < argc && argv[idx] != 0) { if (argv[idx][0] == '-') { c = isvalidopt(argv[idx][1], options, &takesarg); - if (c == '?') + + if (c == '?') { return 1; + } + tmp_argv[tmpidx] = argv[idx]; tmpidx++; + if (takesarg) { - tmp_argv[tmpidx] = argv[idx+1]; + tmp_argv[tmpidx] = argv[idx + 1]; // printf("tmp_argv[%d] = %s\n", tmpidx, tmp_argv[tmpidx]); tmpidx++; idx++; } } + idx++; } // Add non-options to the end idx = 1; + while (idx < argc && argv[idx] != 0) { if (argv[idx][0] == '-') { c = isvalidopt(argv[idx][1], options, &takesarg); - if (c == '?') + + if (c == '?') { return c; + } + if (takesarg) { idx++; } - } - else { + + } else { tmp_argv[tmpidx] = argv[idx]; tmpidx++; } + idx++; } // Reorder argv - for (idx=1; idx < argc; idx++) { + for (idx = 1; idx < argc; idx++) { argv[idx] = tmp_argv[idx]; } @@ -128,24 +144,32 @@ __EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myopti int takesarg; if (*myoptind == 1) - if (reorder(argc, argv, options) != 0) + if (reorder(argc, argv, options) != 0) { return (int)'?'; + } p = argv[*myoptind]; - if (*myoptarg == 0) + if (*myoptarg == 0) { *myoptarg = argv[*myoptind]; + } if (p && options && myoptind && p[0] == '-') { c = isvalidopt(p[1], options, &takesarg); - if (c == '?') + + if (c == '?') { return (int)c; + } + *myoptind += 1; + if (takesarg) { *myoptarg = argv[*myoptind]; *myoptind += 1; } + return (int)c; } + return -1; } diff --git a/src/platforms/nuttx/CMakeLists.txt b/src/platforms/nuttx/CMakeLists.txt new file mode 100644 index 000000000000..37c2ac48791e --- /dev/null +++ b/src/platforms/nuttx/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__nuttx + COMPILE_FLAGS + -Os + SRCS + px4_nuttx_impl.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/nuttx/px4_layer/CMakeLists.txt b/src/platforms/nuttx/px4_layer/CMakeLists.txt new file mode 100644 index 000000000000..86dc1ebb4825 --- /dev/null +++ b/src/platforms/nuttx/px4_layer/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__nuttx__px4_layer + COMPILE_FLAGS + -Os + SRCS + px4_nuttx_tasks.c + ../../posix/px4_layer/px4_log.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/nuttx/px4_layer/module.mk b/src/platforms/nuttx/px4_layer/module.mk index 1738063a56f5..93b55e67ab44 100644 --- a/src/platforms/nuttx/px4_layer/module.mk +++ b/src/platforms/nuttx/px4_layer/module.mk @@ -35,6 +35,7 @@ # NuttX porting layer # -SRCS = px4_nuttx_tasks.c +SRCS = px4_nuttx_tasks.c \ + ../../posix/px4_layer/px4_log.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c index e9b722de4c47..bb4dea7d4ab9 100644 --- a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c +++ b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c @@ -50,7 +50,9 @@ #include #include +#ifndef CONFIG_ARCH_BOARD_SIM #include +#endif #include @@ -61,18 +63,21 @@ void px4_systemreset(bool to_bootloader) { if (to_bootloader) { +#ifndef CONFIG_ARCH_BOARD_SIM stm32_pwr_enablebkp(); +#endif /* XXX wow, this is evil - write a magic number into backup register zero */ *(uint32_t *)0x40002850 = 0xb007b007; } + up_systemreset(); /* lock up here */ - while(true); + while (true); } -int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[]) +int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char *const argv[]) { int pid; diff --git a/src/platforms/posix/drivers/accelsim/CMakeLists.txt b/src/platforms/posix/drivers/accelsim/CMakeLists.txt new file mode 100644 index 000000000000..afa59e515f12 --- /dev/null +++ b/src/platforms/posix/drivers/accelsim/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__drivers__accelsim + MAIN accelsim + SRCS + accelsim.cpp + DEPENDS + platforms__common + git_jmavsim + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index d0f4af0b0fbe..5017bb111ced 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -41,19 +41,16 @@ #include #include #include -#include #include #include -#include #include -#include #include -#include -#include #include #include #include +#include + #include #include @@ -62,7 +59,6 @@ #include #include #include -#include #include #include @@ -86,6 +82,8 @@ static const int ERROR = -1; #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) +#define ACC_READ (0<<6) +#define MAG_READ (1<<6) extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); } @@ -95,7 +93,7 @@ class ACCELSIM_mag; class ACCELSIM : public device::VDev { public: - ACCELSIM(const char* path, enum Rotation rotation); + ACCELSIM(const char *path, enum Rotation rotation); virtual ~ACCELSIM(); virtual int init(); @@ -103,11 +101,6 @@ class ACCELSIM : public device::VDev virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - /** - * Diagnostics - print some basic information about the driver. - */ - //void print_info(); - /** * dump register values */ @@ -116,8 +109,8 @@ class ACCELSIM : public device::VDev protected: friend class ACCELSIM_mag; - virtual ssize_t mag_read(device::file_t *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(device::file_t *filp, int cmd, unsigned long arg); + ssize_t mag_read(device::file_t *filp, char *buffer, size_t buflen); + int mag_ioctl(device::file_t *filp, int cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); private: @@ -173,10 +166,6 @@ class ACCELSIM : public device::VDev // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define ACCELSIM_NUM_CHECKED_REGISTERS 1 - static const uint8_t _checked_registers[ACCELSIM_NUM_CHECKED_REGISTERS]; - uint8_t _checked_values[ACCELSIM_NUM_CHECKED_REGISTERS]; - uint8_t _checked_next; /** * Start automatic measurement. @@ -223,41 +212,6 @@ class ACCELSIM : public device::VDev */ void mag_measure(); - /** - * Read a register from the ACCELSIM - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - - /** - * Write a register in the ACCELSIM - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the ACCELSIM - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Write a register in the ACCELSIM, updating _checked_values - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_checked_reg(unsigned reg, uint8_t value); - /** * Set the ACCELSIM accel measurement range. * @@ -306,16 +260,10 @@ class ACCELSIM : public device::VDev int mag_set_samplerate(unsigned frequency); /* this class cannot be copied */ - ACCELSIM(const ACCELSIM&); - ACCELSIM operator=(const ACCELSIM&); + ACCELSIM(const ACCELSIM &); + ACCELSIM operator=(const ACCELSIM &); }; -/* - list of registers that will be checked in check_registers(). Note - that ADDR_WHO_AM_I must be first in the list. - */ -const uint8_t ACCELSIM::_checked_registers[ACCELSIM_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I }; - /** * Helper class implementing the mag driver node. */ @@ -325,33 +273,32 @@ class ACCELSIM_mag : public device::VDev ACCELSIM_mag(ACCELSIM *parent); ~ACCELSIM_mag(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - virtual int init(); + virtual int init(); protected: friend class ACCELSIM; - void parent_poll_notify(); private: - ACCELSIM *_parent; + ACCELSIM *_parent; - orb_advert_t _mag_topic; - int _mag_orb_class_instance; - int _mag_class_instance; + orb_advert_t _mag_topic; + int _mag_orb_class_instance; + int _mag_class_instance; - void measure(); + void measure(); - void measure_trampoline(void *arg); + void measure_trampoline(void *arg); /* this class does not allow copying due to ptr data members */ - ACCELSIM_mag(const ACCELSIM_mag&); - ACCELSIM_mag operator=(const ACCELSIM_mag&); + ACCELSIM_mag(const ACCELSIM_mag &); + ACCELSIM_mag operator=(const ACCELSIM_mag &); }; -ACCELSIM::ACCELSIM(const char* path, enum Rotation rotation) : +ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : VDev("ACCELSIM", path), _mag(new ACCELSIM_mag(this)), _accel_call{}, @@ -384,16 +331,13 @@ ACCELSIM::ACCELSIM(const char* path, enum Rotation rotation) : _accel_filter_z(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _constant_accel_count(0), - _last_temperature(0), - _checked_next(0) + _last_temperature(0) { - - // enable debug() calls _debug_enabled = false; _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; - + /* Prime _mag with parents devid. */ _mag->_device_id.devid = _device_id.devid; _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; @@ -421,13 +365,17 @@ ACCELSIM::~ACCELSIM() stop(); /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_mag_reports != nullptr) + } + + if (_mag_reports != nullptr) { delete _mag_reports; + } - if (_accel_class_instance != -1) + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } delete _mag; @@ -444,6 +392,9 @@ ACCELSIM::init() { int ret = ERROR; + struct mag_report mrp = {}; + struct accel_report arp = {}; + /* do SIM init first */ if (VDev::init() != OK) { PX4_WARN("SIM init failed"); @@ -453,18 +404,21 @@ ACCELSIM::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + if (_accel_reports == nullptr) { goto out; + } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - if (_mag_reports == nullptr) + if (_mag_reports == nullptr) { goto out; + } reset(); /* do VDev init for the mag device node */ ret = _mag->init(); + if (ret != OK) { PX4_WARN("MAG init failed"); goto out; @@ -474,12 +428,11 @@ ACCELSIM::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct mag_report mrp; _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic == nullptr) { PX4_WARN("ADVERT ERR"); @@ -489,12 +442,11 @@ ACCELSIM::init() _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, ORB_PRIO_DEFAULT); + &_accel_orb_class_instance, ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { PX4_WARN("ADVERT ERR"); @@ -512,6 +464,28 @@ ACCELSIM::reset() int ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) { + uint8_t cmd = send[0]; + + if (cmd & DIR_READ) { + // Get data from the simulator + Simulator *sim = Simulator::getInstance(); + + if (sim == NULL) { + return ENODEV; + } + + // FIXME - not sure what interrupt status should be + recv[1] = 0; + + // skip cmd and status bytes + if (cmd & ACC_READ) { + sim->getRawAccelReport(&recv[2], len - 2); + + } else if (cmd & MAG_READ) { + sim->getMagReport(&recv[2], len - 2); + } + } + return PX4_OK; } @@ -523,8 +497,9 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_accel_interval > 0) { @@ -546,8 +521,9 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(arb)) + if (_accel_reports->get(arb)) { ret = sizeof(*arb); + } return ret; } @@ -560,8 +536,9 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_mag_interval > 0) { @@ -585,8 +562,9 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) _mag->measure(); /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(mrb)) + if (_mag_reports->get(mrb)) { ret = sizeof(*mrb); + } return ret; } @@ -597,7 +575,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -619,51 +597,55 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + /* convert hz to hrt interval via microseconds */ + unsigned period = 1000000 / arg; - /* check against maximum sane rate */ - if (ticks < 500) - return -EINVAL; + /* check against maximum sane rate */ + if (period < 500) { + return -EINVAL; + } - /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + /* adjust filters */ + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = ticks; + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = period; - /* 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; + return OK; + } } } - } case SENSORIOCGPOLLRATE: - if (_call_accel_interval == 0) + if (_call_accel_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_accel_interval; 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; + } - if (!_accel_reports->resize(arg)) { - return -ENOMEM; - } + if (!_accel_reports->resize(arg)) { + return -ENOMEM; + } - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -679,20 +661,22 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return _accel_samplerate; case ACCELIOCSLOWPASS: { - return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); - } + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + } case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - } else { - return -EINVAL; + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + + } else { + return -EINVAL; + } } - } case ACCELIOCSRANGE: /* arg needs to be in G */ @@ -700,7 +684,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ - return (unsigned long)((_accel_range_m_s2)/ACCELSIM_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / ACCELSIM_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -722,7 +706,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -749,19 +733,23 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_call_mag_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned period = 1000000 / arg; - /* check against maximum sane rate */ - if (ticks < 1000) + /* check against maximum sane rate (1ms) */ + if (period < 10000) { return -EINVAL; + } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _mag_call.period = _call_mag_interval = ticks; + _mag_call.period = _call_mag_interval = period; + + //PX4_INFO("SET _call_mag_interval=%u", _call_mag_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -769,22 +757,24 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_mag_interval == 0) + if (_call_mag_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_mag_reports->resize(arg)) { - return -ENOMEM; - } + if (!_mag_reports->resize(arg)) { + return -ENOMEM; + } - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _mag_reports->size(); @@ -829,58 +819,13 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) case MAGIOCSELFTEST: return OK; + default: /* give it to the superclass */ return VDev::ioctl(filp, cmd, arg); } } -uint8_t -ACCELSIM::read_reg(unsigned reg) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; - cmd[1] = 0; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -void -ACCELSIM::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - -void -ACCELSIM::write_checked_reg(unsigned reg, uint8_t value) -{ - write_reg(reg, value); - for (uint8_t i=0; iflush(); /* start polling at the specified rate */ + //PX4_INFO("ACCELSIM::start accel %u", _call_accel_interval); hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&ACCELSIM::measure_trampoline, this); + + // There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but _call_mag_interval is 0 + if (_call_mag_interval == 0) { + //PX4_INFO("_call_mag_interval uninitilized - would have set period delay of 0"); + _call_mag_interval = 10000; // Max 100Hz + } + + //PX4_INFO("ACCELSIM::start mag %u", _call_mag_interval); hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&ACCELSIM::mag_measure_trampoline, this); } @@ -948,6 +902,7 @@ ACCELSIM::stop() void ACCELSIM::measure_trampoline(void *arg) { + //PX4_INFO("ACCELSIM::measure_trampoline"); ACCELSIM *dev = (ACCELSIM *)arg; /* make another measurement */ @@ -957,6 +912,7 @@ ACCELSIM::measure_trampoline(void *arg) void ACCELSIM::mag_measure_trampoline(void *arg) { + //PX4_INFO("ACCELSIM::mag_measure_trampoline"); ACCELSIM *dev = (ACCELSIM *)arg; /* make another measurement */ @@ -972,21 +928,25 @@ ACCELSIM::measure() struct { uint8_t cmd; uint8_t status; - int16_t x; - int16_t y; - int16_t z; + float temperature; + float x; + float y; + float z; } raw_accel_report; #pragma pack(pop) - accel_report accel_report; + accel_report accel_report = {}; /* start the performance counter */ perf_begin(_accel_sample_perf); /* fetch data from the sensor */ memset(&raw_accel_report, 0, sizeof(raw_accel_report)); - raw_accel_report.cmd = DIR_READ; - transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + raw_accel_report.cmd = DIR_READ | ACC_READ; + + if (OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) { + return; + } /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1013,56 +973,15 @@ ACCELSIM::measure() // register reads and bad values. This allows the higher level // code to decide if it should use this sensor based on // whether it has had failures - accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - - accel_report.x_raw = raw_accel_report.x; - accel_report.y_raw = raw_accel_report.y; - accel_report.z_raw = raw_accel_report.z; + accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - float xraw_f = raw_accel_report.x; - float yraw_f = raw_accel_report.y; - float zraw_f = raw_accel_report.z; - - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + accel_report.x_raw = (int16_t)(raw_accel_report.x / _accel_range_scale); + accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale); + accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale); - float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - - /* - we have logs where the accelerometers get stuck at a fixed - large value. We want to detect this and mark the sensor as - being faulty - */ - if (fabsf(_last_accel[0] - x_in_new) < 0.001f && - fabsf(_last_accel[1] - y_in_new) < 0.001f && - fabsf(_last_accel[2] - z_in_new) < 0.001f && - fabsf(x_in_new) > 20 && - fabsf(y_in_new) > 20 && - fabsf(z_in_new) > 20) { - _constant_accel_count += 1; - } else { - _constant_accel_count = 0; - } - if (_constant_accel_count > 100) { - // we've had 100 constant accel readings with large - // values. The sensor is almost certainly dead. We - // will raise the error_count so that the top level - // flight code will know to avoid this sensor, but - // we'll still give the data so that it can be logged - // and viewed - perf_count(_bad_values); - _constant_accel_count = 0; - } - - _last_accel[0] = x_in_new; - _last_accel[1] = y_in_new; - _last_accel[2] = z_in_new; - - accel_report.x = _accel_filter_x.apply(x_in_new); - accel_report.y = _accel_filter_y.apply(y_in_new); - accel_report.z = _accel_filter_z.apply(z_in_new); + accel_report.x = raw_accel_report.x; + accel_report.y = raw_accel_report.y; + accel_report.z = raw_accel_report.z; accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1095,11 +1014,11 @@ ACCELSIM::mag_measure() #pragma pack(push, 1) struct { uint8_t cmd; - int16_t temperature; uint8_t status; - int16_t x; - int16_t y; - int16_t z; + float temperature; + float x; + float y; + float z; } raw_mag_report; #pragma pack(pop) @@ -1111,8 +1030,11 @@ ACCELSIM::mag_measure() /* fetch data from the sensor */ memset(&raw_mag_report, 0, sizeof(raw_mag_report)); - raw_mag_report.cmd = DIR_READ; - transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + raw_mag_report.cmd = DIR_READ | MAG_READ; + + if (OK != transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report))) { + return; + } /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1132,29 +1054,26 @@ ACCELSIM::mag_measure() mag_report.timestamp = hrt_absolute_time(); - mag_report.x_raw = raw_mag_report.x; - mag_report.y_raw = raw_mag_report.y; - mag_report.z_raw = raw_mag_report.z; + mag_report.x_raw = (int16_t)(raw_mag_report.x / _mag_range_scale); + mag_report.y_raw = (int16_t)(raw_mag_report.y / _mag_range_scale); + mag_report.z_raw = (int16_t)(raw_mag_report.z / _mag_range_scale); + + float xraw_f = (int16_t)(raw_mag_report.x / _mag_range_scale); + float yraw_f = (int16_t)(raw_mag_report.y / _mag_range_scale); + float zraw_f = (int16_t)(raw_mag_report.z / _mag_range_scale); - float xraw_f = mag_report.x_raw; - float yraw_f = mag_report.y_raw; - float zraw_f = mag_report.z_raw; /* apply user specified rotation */ rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; - mag_report.scaling = _mag_range_scale; - mag_report.range_ga = (float)_mag_range_ga; - mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - /* remember the temperature. The datasheet isn't clear, but it * seems to be a signed offset from 25 degrees C in units of 0.125C */ - _last_temperature = 25 + (raw_mag_report.temperature * 0.125f); + _last_temperature = raw_mag_report.temperature; mag_report.temperature = _last_temperature; + mag_report.x = raw_mag_report.x; + mag_report.y = raw_mag_report.y; + mag_report.z = raw_mag_report.z; _mag_reports->force(&mag_report); @@ -1183,8 +1102,9 @@ ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : ACCELSIM_mag::~ACCELSIM_mag() { - if (_mag_class_instance != -1) + if (_mag_class_instance != -1) { unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); + } } int @@ -1193,8 +1113,10 @@ ACCELSIM_mag::init() int ret; ret = VDev::init(); - if (ret != OK) + + if (ret != OK) { goto out; + } _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); @@ -1202,12 +1124,6 @@ ACCELSIM_mag::init() return ret; } -void -ACCELSIM_mag::parent_poll_notify() -{ - poll_notify(POLLIN); -} - ssize_t ACCELSIM_mag::read(device::file_t *filp, char *buffer, size_t buflen) { @@ -1218,11 +1134,12 @@ int ACCELSIM_mag::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - case DEVIOCGDEVICEID: - return (int)VDev::ioctl(filp, cmd, arg); - break; - default: - return _parent->mag_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->mag_ioctl(filp, cmd, arg); } } @@ -1260,8 +1177,9 @@ int start(enum Rotation rotation) { int fd, fd_mag; + if (g_dev != nullptr) { - PX4_WARN( "already started"); + PX4_WARN("already started"); return 0; } @@ -1288,7 +1206,7 @@ start(enum Rotation rotation) if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); - px4_close(fd); + px4_close(fd); goto fail; } @@ -1299,14 +1217,13 @@ start(enum Rotation rotation) if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } - } - else - { + + } else { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } - px4_close(fd); - px4_close(fd_mag); + px4_close(fd); + px4_close(fd_mag); return 0; fail: @@ -1332,7 +1249,6 @@ info() } PX4_DEBUG("state @ %p", g_dev); - //g_dev->print_info(); return 0; } @@ -1354,7 +1270,7 @@ accelsim_main(int argc, char *argv[]) enum Rotation rotation = ROTATION_NONE; int ret; int myoptind = 1; - const char * myoptarg = NULL; + const char *myoptarg = NULL; /* jump over start/off/etc and look at options first */ while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { @@ -1362,6 +1278,7 @@ accelsim_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(myoptarg); break; + default: accelsim::usage(); return 0; @@ -1373,18 +1290,21 @@ accelsim_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { ret = accelsim::start(rotation); + } /* * Print driver information. */ - else if (!strcmp(verb, "info")) + else if (!strcmp(verb, "info")) { ret = accelsim::info(); + } else { accelsim::usage(); return 1; } + return ret; } diff --git a/src/platforms/posix/drivers/adcsim/CMakeLists.txt b/src/platforms/posix/drivers/adcsim/CMakeLists.txt new file mode 100644 index 000000000000..93cdd95a0f34 --- /dev/null +++ b/src/platforms/posix/drivers/adcsim/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__drivers__adcsim + MAIN adcsim + COMPILE_FLAGS + -Os + SRCS + adcsim.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index fa6690ac21fd..c72cc1a4940a 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -37,7 +37,7 @@ * Driver for the ADCSIM. * * This is a designed for simulating sampling things like voltages - * and so forth. + * and so forth. */ #include @@ -83,7 +83,7 @@ class ADCSIM : public device::VDev private: static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - + hrt_call _call; perf_counter_t _sample_perf; @@ -111,6 +111,7 @@ class ADCSIM : public device::VDev ADCSIM::ADCSIM(uint32_t channels) : VDev("adcsim", ADCSIM0_DEVICE_PATH), + _call(), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) @@ -126,11 +127,13 @@ ADCSIM::ADCSIM(uint32_t channels) : _channel_count++; } } + _samples = new adc_msg_s[_channel_count]; /* prefill the channel numbers in the sample array */ if (_samples != nullptr) { unsigned index = 0; + for (unsigned i = 0; i < 32; i++) { if (channels & (1 << i)) { _samples[index].am_channel = i; @@ -143,14 +146,15 @@ ADCSIM::ADCSIM(uint32_t channels) : ADCSIM::~ADCSIM() { - if (_samples != nullptr) + if (_samples != nullptr) { delete _samples; + } } int ADCSIM::init() { - debug("init done"); + DEVICE_DEBUG("init done"); /* create the device node */ return VDev::init(); @@ -167,8 +171,9 @@ ADCSIM::read(device::file_t *filp, char *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; - if (len > maxsize) + if (len > maxsize) { len = maxsize; + } /* block interrupts while copying samples to avoid racing with an update */ memcpy(buffer, _samples, len); @@ -205,8 +210,10 @@ void ADCSIM::_tick() { /* scan the channel set and sample each */ - for (unsigned i = 0; i < _channel_count; i++) + for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); + } + update_system_power(); } @@ -240,6 +247,7 @@ test(void) { int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY); + if (fd < 0) { PX4_ERR("can't open ADCSIM device"); return 1; @@ -274,7 +282,7 @@ adcsim_main(int argc, char *argv[]) int ret = 0; if (g_adc == nullptr) { - /* XXX this hardcodes the default channel set for POSIXTEST - should be configurable */ + /* FIXME - this hardcodes the default channel set for SITL - should be configurable */ g_adc = new ADCSIM((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); if (g_adc == nullptr) { @@ -290,8 +298,9 @@ adcsim_main(int argc, char *argv[]) } if (argc > 1) { - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { ret = test(); + } } return ret; diff --git a/src/platforms/posix/drivers/airspeedsim/CMakeLists.txt b/src/platforms/posix/drivers/airspeedsim/CMakeLists.txt new file mode 100644 index 000000000000..8c2a410862da --- /dev/null +++ b/src/platforms/posix/drivers/airspeedsim/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__drivers__airspeedsim + MAIN measairspeedsim + COMPILE_FLAGS + -Os + SRCS + airspeedsim.cpp + meas_airspeed_sim.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index befb0b39a595..58f6aa9929f1 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -35,14 +35,12 @@ * @file ets_airspeed.cpp * @author Simon Wilks * @author Mark Charlebois (simulator) - * - * Driver for the Simulated Airspeed Sensor based on Eagle Tree Airspeed V3. + * @author Roman Bapst (simulator) + * Driver for the Simulated AirspeedSim Sensor based on Eagle Tree AirspeedSim V3. */ #include -#include - #include #include #include @@ -56,8 +54,6 @@ #include #include -#include - #include #include #include @@ -71,12 +67,15 @@ #include #include -#include +#include + +#include "airspeedsim.h" -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : - I2C("Airspeed", path, bus, address), +AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path) : + VDev("AIRSPEEDSIM", path), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), + _retries(0), _max_differential_pressure_pa(0), _sensor_ok(false), _last_published_sensor_ok(true), /* initialize differently to force publication */ @@ -97,17 +96,19 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha memset(&_work, 0, sizeof(_work)); } -Airspeed::~Airspeed() +AirspeedSim::~AirspeedSim() { /* make sure we are truly inactive */ stop(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -116,18 +117,22 @@ Airspeed::~Airspeed() } int -Airspeed::init() +AirspeedSim::init() { int ret = ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) + /* init base class */ + if (VDev::init() != OK) { + DEVICE_DEBUG("VDev init failed"); goto out; + } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); - if (_reports == nullptr) + + if (_reports == nullptr) { goto out; + } /* register alternate interfaces if we have to */ _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); @@ -143,8 +148,9 @@ Airspeed::init() /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); - if (_airspeed_pub == nullptr) + if (_airspeed_pub == nullptr) { PX4_WARN("uORB started?"); + } } ret = OK; @@ -154,7 +160,7 @@ Airspeed::init() } int -Airspeed::probe() +AirspeedSim::probe() { /* on initial power up the device may need more than one retry for detection. Once it is running the number of retries can @@ -163,33 +169,33 @@ Airspeed::probe() _retries = 4; int ret = measure(); - // drop back to 2 retries once initialised + // drop back to 2 retries once initialised _retries = 2; return ret; } int -Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) +AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -199,30 +205,33 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* 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 */ - long ticks = USEC2TICK(1000000 / arg); + unsigned long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(_conversion_interval)) + if (ticks < USEC2TICK(_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) + if (want_start) { start(); + } return OK; } @@ -230,21 +239,24 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) } 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)) + if ((arg < 1) || (arg > 100)) { return -EINVAL; + } //irqstate_t flags = irqsave(); if (!_reports->resize(arg)) { //irqrestore(flags); return -ENOMEM; } + //irqrestore(flags); return OK; @@ -258,34 +270,36 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) return -EINVAL; case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - _diff_pres_offset = s->offset_pa; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + _diff_pres_offset = s->offset_pa; + return OK; } case AIRSPEEDIOCGSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - s->offset_pa = _diff_pres_offset; - s->scale = 1.0f; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + s->offset_pa = _diff_pres_offset; + s->scale = 1.0f; + return OK; } default: /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); + //return I2C::ioctl(filp, cmd, arg); XXX SIM should be super class + return 0; } } ssize_t -Airspeed::read(device::file_t *filp, char *buffer, size_t buflen) +AirspeedSim::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(differential_pressure_s); differential_pressure_s *abuf = 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) { @@ -336,24 +350,24 @@ Airspeed::read(device::file_t *filp, char *buffer, size_t buflen) } void -Airspeed::start() +AirspeedSim::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)&Airspeed::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&AirspeedSim::cycle_trampoline, this, 1); } void -Airspeed::stop() +AirspeedSim::stop() { work_cancel(HPWORK, &_work); } void -Airspeed::update_status() +AirspeedSim::update_status() { if (_sensor_ok != _last_published_sensor_ok) { /* notify about state change */ @@ -366,6 +380,7 @@ Airspeed::update_status() if (_subsys_pub != nullptr) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -375,9 +390,9 @@ Airspeed::update_status() } void -Airspeed::cycle_trampoline(void *arg) +AirspeedSim::cycle_trampoline(void *arg) { - Airspeed *dev = (Airspeed *)arg; + AirspeedSim *dev = (AirspeedSim *)arg; dev->cycle(); // XXX we do not know if this is @@ -387,7 +402,7 @@ Airspeed::cycle_trampoline(void *arg) } void -Airspeed::print_info() +AirspeedSim::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -397,8 +412,31 @@ Airspeed::print_info() } void -Airspeed::new_report(const differential_pressure_s &report) +AirspeedSim::new_report(const differential_pressure_s &report) { - if (!_reports->force(&report)) + if (!_reports->force(&report)) { perf_count(_buffer_overflows); + } } + +int +AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + if (recv_len > 0) { + // this is equivalent to the collect phase + Simulator *sim = Simulator::getInstance(); + + if (sim == NULL) { + PX4_ERR("Error BARO_SIM::transfer no simulator"); + return -ENODEV; + } + + PX4_DEBUG("BARO_SIM::transfer getting sample"); + sim->getAirspeedSample(recv, recv_len); + + } else { + // we don't need measure phase + } + + return 0; +} \ No newline at end of file diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h index 66f4c58627c2..91db618d9b27 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h @@ -40,7 +40,7 @@ #include -#include +//#include #include #include @@ -55,10 +55,11 @@ #include #include -#include -#include -#include +//#include +//#include +//#include +#include #include #include @@ -69,6 +70,7 @@ #include #include #include +#include #include #include @@ -87,7 +89,7 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class __EXPORT AirspeedSim : public device::I2C +class __EXPORT AirspeedSim : public device::VDev { public: AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path); @@ -95,8 +97,8 @@ class __EXPORT AirspeedSim : public device::I2C virtual int init(); - virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. @@ -104,9 +106,11 @@ class __EXPORT AirspeedSim : public device::I2C virtual void print_info(); private: - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; perf_counter_t _buffer_overflows; + unsigned _retries; // XXX this should come from the SIM class + /* this class has pointer data members and should not be copied */ AirspeedSim(const AirspeedSim &); AirspeedSim &operator=(const AirspeedSim &); @@ -122,16 +126,19 @@ class __EXPORT AirspeedSim : public device::I2C virtual int measure() = 0; virtual int collect() = 0; + virtual int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); + /** * Update the subsystem status */ void update_status(); - work_s _work; + struct work_s _work; float _max_differential_pressure_pa; bool _sensor_ok; bool _last_published_sensor_ok; - int _measure_ticks; + unsigned _measure_ticks; bool _collect_phase; float _diff_pres_offset; diff --git a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp new file mode 100644 index 000000000000..ae43a2cd5d26 --- /dev/null +++ b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp @@ -0,0 +1,625 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file meas_airspeed_sim.cpp + * @author Lorenz Meier + * @author Sarthak Kaingade + * @author Simon Wilks + * @author Thomas Gubler + * @author Roman Bapst + * + * Driver for a simulated airspeed sensor. + * + */ + + +#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 "airspeedsim.h" + +/* I2C bus address is 1010001x */ +#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */ +#define PATH_MS4525 "/dev/ms4525" +/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ +#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ +#define PATH_MS5525 "/dev/ms5525" + +/* Register address */ +#define ADDR_READ_MR 0x00 /* write to this address to start conversion */ + +/* Measurement rate is 100Hz */ +#define MEAS_RATE 100 +#define MEAS_DRIVER_FILTER_FREQ 1.2f +#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ + +class MEASAirspeedSim : public AirspeedSim +{ +public: + MEASAirspeedSim(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525); + +protected: + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + virtual void cycle(); + virtual int measure(); + virtual int collect(); + + math::LowPassFilter2p _filter; + + /** + * Correct for 5V rail voltage variations + */ + void voltage_correction(float &diff_pres_pa, float &temperature); + + int _t_system_power; + struct system_power_s system_power; +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int measairspeedsim_main(int argc, char *argv[]); + +MEASAirspeedSim::MEASAirspeedSim(int bus, int address, const char *path) : AirspeedSim(bus, address, + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), + _t_system_power(-1), + system_power{} +{} + +int +MEASAirspeedSim::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = 0; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return ret; +} + +int +MEASAirspeedSim::collect() +{ + int ret = -EIO; + + /* read from the sensor */ +#pragma pack(push, 1) + struct { + float temperature; + float diff_pressure; + } airspeed_report; +#pragma pack(pop) + + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, (uint8_t *)&airspeed_report, sizeof(airspeed_report)); + + if (ret < 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint8_t status = 0; + + switch (status) { + case 0: + break; + + case 1: + + /* fallthrough */ + case 2: + + /* fallthrough */ + case 3: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + } + + float temperature = airspeed_report.temperature; + float diff_press_pa_raw = airspeed_report.diff_pressure * 100.0f; // convert from millibar to bar + + struct differential_pressure_s report; + + /* track maximum differential pressure measured (so we can work out top speed). */ + if (diff_press_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa_raw; + } + + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.temperature = temperature; + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); + + report.differential_pressure_raw_pa = diff_press_pa_raw; + report.max_differential_pressure_pa = _max_differential_pressure_pa; + + if (_airspeed_pub != nullptr && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } + + new_report(report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + + return ret; +} + +void +MEASAirspeedSim::cycle() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + + if (OK != ret) { + /* restart the measurement state machine */ + start(); + _sensor_ok = false; + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&AirspeedSim::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + + if (OK != ret) { + DEVICE_DEBUG("measure error"); + } + + _sensor_ok = (ret == OK); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&AirspeedSim::cycle_trampoline, + this, + USEC2TICK(CONVERSION_INTERVAL)); +} + +/** + correct for 5V rail voltage if the system_power ORB topic is + available + + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of + offset versus voltage for 3 sensors + */ +void +MEASAirspeedSim::voltage_correction(float &diff_press_pa, float &temperature) +{ +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + + if (_t_system_power == -1) { + _t_system_power = orb_subscribe(ORB_ID(system_power)); + } + + if (_t_system_power == -1) { + // not available + return; + } + + bool updated = false; + orb_check(_t_system_power, &updated); + + if (updated) { + orb_copy(ORB_ID(system_power), _t_system_power, &system_power); + } + + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { + // not valid, skip correction + return; + } + + const float slope = 65.0f; + /* + apply a piecewise linear correction, flattening at 0.5V from 5V + */ + float voltage_diff = system_power.voltage5V_v - 5.0f; + + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + + if (voltage_diff < -0.5f) { + voltage_diff = -0.5f; + } + + diff_press_pa -= voltage_diff * slope; + + /* + the temperature masurement varies as well + */ + const float temp_slope = 0.887f; + voltage_diff = system_power.voltage5V_v - 5.0f; + + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + + if (voltage_diff < -1.0f) { + voltage_diff = -1.0f; + } + + temperature -= voltage_diff * temp_slope; +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +} + +/** + * Local functions in support of the shell command. + */ +namespace meas_airspeed_sim +{ + +MEASAirspeedSim *g_dev = nullptr; + +int start(int i2c_bus); +int stop(); +int test(); +int reset(); +int info(); + +/** + * Start the driver. + * + * This function call only returns once the driver is up and running + * or failed to detect the sensor. + */ +int +start(int i2c_bus) +{ + int fd; + + if (g_dev != nullptr) { + PX4_WARN("already started"); + } + + /* create the driver, try the MS4525DO first */ + g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); + + /* check if the MS4525DO was instantiated */ + if (g_dev == nullptr) { + goto fail; + } + + /* try the MS5525DSO next if init fails */ + if (OK != g_dev->AirspeedSim::init()) { + delete g_dev; + g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); + + /* check if the MS5525DSO was instantiated */ + if (g_dev == nullptr) { + goto fail; + } + + /* both versions failed if the init for the MS5525DSO fails, give up */ + if (OK != g_dev->AirspeedSim::init()) { + goto fail; + } + } + + /* set the poll rate to default, starts automatic data collection */ + fd = px4_open(PATH_MS4525, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + return 0; + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + PX4_ERR("no MS4525 airspeedSim sensor connected"); + return 1; +} + +/** + * Stop the driver + */ +int +stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + PX4_ERR("driver not running"); + } + + return 0; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +int +test() +{ + struct differential_pressure_s report; + ssize_t sz; + int ret; + + int fd = px4_open(PATH_MS4525, O_RDONLY); + + if (fd < 0) { + PX4_ERR("%s open failed (try 'meas_airspeed_sim start' if the driver is not running", PATH_MS4525); + return 1; + } + + /* do a simple demand read */ + sz = px4_read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_ERR("immediate read failed"); + return 1; + } + + PX4_WARN("single read"); + PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); + + /* start the sensor polling at 2Hz */ + if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + PX4_WARN("failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + warnx("timed out"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_WARN("periodic read failed"); + } + + PX4_WARN("periodic read %u", i); + PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); + PX4_WARN("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); + } + + /* reset the sensor polling to its default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + PX4_ERR("failed to set default rate"); + return 1; + } + + PX4_WARN("PASS"); + + return 0; +} + +/** + * Reset the driver. + */ +int +reset() +{ + int fd = open(PATH_MS4525, O_RDONLY); + + if (fd < 0) { + PX4_ERR("failed "); + return 1; + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("driver reset failed"); + return 1; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("driver poll restart failed"); + return 1; + } + + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_WARN("driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return 0; +} + +} // namespace + + +static void +meas_airspeed_usage() +{ + PX4_WARN("usage: meas_airspeed_sim command [options]"); + PX4_WARN("options:"); + PX4_WARN("\t-b --bus i2cbus (%d)", 1); + PX4_WARN("command:"); + PX4_WARN("\tstart|stop|reset|test|info"); +} + +int +measairspeedsim_main(int argc, char *argv[]) +{ + int i2c_bus = 1;//PX4_I2C_BUS_DEFAULT; + + int i; + + for (i = 1; i < argc; i++) { + if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { + if (argc > i + 1) { + i2c_bus = atoi(argv[i + 1]); + } + } + } + + int ret = 0; + + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + ret = meas_airspeed_sim::start(i2c_bus); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + ret = meas_airspeed_sim::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + ret = meas_airspeed_sim::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + ret = meas_airspeed_sim::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + ret = meas_airspeed_sim::info(); + } + + meas_airspeed_usage(); + return ret; +} diff --git a/src/platforms/posix/drivers/airspeedsim/module.mk b/src/platforms/posix/drivers/airspeedsim/module.mk index 2d971e4d2808..fb12fa04a2be 100644 --- a/src/platforms/posix/drivers/airspeedsim/module.mk +++ b/src/platforms/posix/drivers/airspeedsim/module.mk @@ -34,7 +34,8 @@ # # Makefile to build the generic airspeed driver. # +MODULE_COMMAND = measairspeedsim -SRCS = airspeedsim.cpp +SRCS = airspeedsim.cpp meas_airspeed_sim.cpp MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/drivers/barosim/CMakeLists.txt b/src/platforms/posix/drivers/barosim/CMakeLists.txt new file mode 100644 index 000000000000..15963f87ebc8 --- /dev/null +++ b/src/platforms/posix/drivers/barosim/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__drivers__barosim + MAIN barosim + COMPILE_FLAGS + -Os + SRCS + baro.cpp + baro_sim.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 90f0f71efce0..975fa3687c21 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -72,12 +72,6 @@ enum BAROSIM_BUS { BAROSIM_BUS_SIM_EXTERNAL }; -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - /* helper macro for handling report buffer indices */ #define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) @@ -95,7 +89,7 @@ static const int ERROR = -1; class BAROSIM : public device::VDev { public: - BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char* path); + BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path); ~BAROSIM(); virtual int init(); @@ -201,7 +195,7 @@ class BAROSIM : public device::VDev */ extern "C" __EXPORT int barosim_main(int argc, char *argv[]); -BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char* path) : +BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path) : VDev("BAROSIM", path), _interface(interface), _prom(prom_buf.s), @@ -230,12 +224,14 @@ BAROSIM::~BAROSIM() /* make sure we are truly inactive */ stop_cycle(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(get_devname(), _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -250,11 +246,12 @@ int BAROSIM::init() { int ret; - debug("BAROSIM::init"); + DEVICE_DEBUG("BAROSIM::init"); ret = VDev::init(); + if (ret != OK) { - debug("VDev init failed"); + DEVICE_DEBUG("VDev init failed"); goto out; } @@ -262,7 +259,7 @@ BAROSIM::init() _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { - debug("can't get memory for reports"); + DEVICE_DEBUG("can't get memory for reports"); ret = -ENOMEM; goto out; } @@ -275,6 +272,13 @@ BAROSIM::init() _measure_phase = 0; _reports->flush(); + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + + if (_baro_topic == nullptr) { + PX4_ERR("failed to create sensor_baro publication"); + } + /* this do..while is goto without goto */ do { /* do temperature first */ @@ -312,12 +316,6 @@ BAROSIM::init() ret = OK; - _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); - - if (_baro_topic == nullptr) { - PX4_ERR("failed to create sensor_baro publication"); - } //PX4_WARN("sensor_baro publication %ld", _baro_topic); } while (0); @@ -334,8 +332,9 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) 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) { @@ -388,8 +387,9 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(brp)) + if (_reports->get(brp)) { ret = sizeof(*brp); + } } while (0); @@ -401,23 +401,23 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - case SENSORIOCSPOLLRATE: + case SENSORIOCSPOLLRATE: switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop_cycle(); _measure_ticks = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -427,51 +427,57 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(BAROSIM_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start_cycle(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* 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); + unsigned long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if ((long)ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(BAROSIM_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) + if (want_start) { start_cycle(); + } 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; + } + + if (!_reports->resize(arg)) { + return -ENOMEM; + } - if (!_reports->resize(arg)) { - return -ENOMEM; + return OK; } - return OK; - } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -486,8 +492,9 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case BAROIOCSMSLPRESSURE: /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) + if ((arg < 80000) || (arg > 120000)) { return -EINVAL; + } _msl_pressure = arg; return OK; @@ -542,6 +549,7 @@ BAROSIM::cycle() /* perform collection */ ret = collect(); + if (ret != OK) { /* issue a reset command to the sensor */ _interface->dev_ioctl(IOCTL_RESET, dummy); @@ -559,7 +567,7 @@ BAROSIM::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - ((long)_measure_ticks > USEC2TICK(BAROSIM_CONVERSION_INTERVAL))) { + (_measure_ticks > USEC2TICK(BAROSIM_CONVERSION_INTERVAL))) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, @@ -574,8 +582,9 @@ BAROSIM::cycle() /* measurement phase */ ret = measure(); + if (ret != OK) { - //log("measure error %d", ret); + //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ _interface->dev_ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ @@ -610,8 +619,10 @@ BAROSIM::measure() * Send the command to begin measuring. */ ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } perf_end(_measure_perf); @@ -622,17 +633,25 @@ int BAROSIM::collect() { int ret; - uint32_t raw; + +#pragma pack(push, 1) + struct { + float pressure; + float altitude; + float temperature; + } baro_report; +#pragma pack(pop) perf_begin(_sample_perf); struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ - ret = _interface->dev_read(0, (void *)&raw, 0); + ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report)); + if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); @@ -641,92 +660,24 @@ BAROSIM::collect() /* handle a measurement */ if (_measure_phase == 0) { - - /* temperature offset (in ADC units) */ - int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8); - - /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ - _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); - - /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); - - /* temperature compensation */ - if (_TEMP < 2000) { - - int32_t T2 = POW2(dT) >> 31; - - int64_t f = POW2((int64_t)_TEMP - 2000); - int64_t OFF2 = 5 * f >> 1; - int64_t SENS2 = 5 * f >> 2; - - if (_TEMP < -1500) { - int64_t f2 = POW2(_TEMP + 1500); - OFF2 += 7 * f2; - SENS2 += 11 * f2 >> 1; - } - - _TEMP -= T2; - _OFF -= OFF2; - _SENS -= SENS2; - } + report.pressure = baro_report.pressure; + report.altitude = baro_report.altitude; + report.temperature = baro_report.temperature; + report.timestamp = hrt_absolute_time(); } else { - - /* pressure calculation, result in Pa */ - int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; - _P = P * 0.01f; - _T = _TEMP * 0.01f; - - /* generate a new report */ - report.temperature = _TEMP / 100.0f; - report.pressure = P / 100.0f; /* convert to millibar */ - - /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ - - /* - * PERFORMANCE HINT: - * - * The single precision calculation is 50 microseconds faster than the double - * precision variant. It is however not obvious if double precision is required. - * Pending more inspection and tests, we'll leave the double precision variant active. - * - * Measurements: - * double precision: barosim_read: 992 events, 258641us elapsed, min 202us max 305us - * single precision: barosim_read: 963 events, 208066us elapsed, min 202us max 241us - */ - - /* tropospheric properties (0-11km) for standard atmosphere */ - const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ - const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ - const double g = 9.80665; /* gravity constant in m/s/s */ - const double R = 287.05; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - double p1 = _msl_pressure / 1000.0; - - /* measured pressure in kPa */ - double p = P / 1000.0; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + report.pressure = baro_report.pressure; + report.altitude = baro_report.altitude; + report.temperature = baro_report.temperature; + report.timestamp = hrt_absolute_time(); /* publish it */ if (!(_pub_blocked)) { if (_baro_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - } - else { + + } else { PX4_WARN("BAROSIM::collect _baro_topic not initialized"); } } @@ -860,6 +811,7 @@ start_bus(struct barosim_bus_option &bus) prom_u prom_buf; device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + if (interface->init() != OK) { delete interface; PX4_ERR("no device on bus %u", (unsigned)bus.busid); @@ -867,13 +819,14 @@ start_bus(struct barosim_bus_option &bus) } bus.dev = new BAROSIM(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; bus.dev = NULL; PX4_ERR("bus init failed %p", bus.dev); return false; } - + int fd = px4_open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ @@ -881,6 +834,7 @@ start_bus(struct barosim_bus_option &bus) PX4_ERR("can't open baro device"); return false; } + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { px4_close(fd); PX4_ERR("failed setting default poll rate"); @@ -931,6 +885,7 @@ test() int fd; fd = px4_open(bus.devpath, O_RDONLY); + if (fd < 0) { PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); return 1; @@ -1008,6 +963,7 @@ reset() int fd; fd = px4_open(bus.devpath, O_RDONLY); + if (fd < 0) { PX4_ERR("failed "); return 1; @@ -1022,6 +978,7 @@ reset() PX4_ERR("driver poll restart failed"); return 1; } + return 0; } @@ -1031,13 +988,15 @@ reset() int info() { - for (uint8_t i=0; iprint_info(); } } + return 0; } @@ -1146,26 +1105,30 @@ barosim_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { ret = barosim::start(); + } /* * Test the driver/device. */ - else if (!strcmp(verb, "test")) + else if (!strcmp(verb, "test")) { ret = barosim::test(); + } /* * Reset the driver. */ - else if (!strcmp(verb, "reset")) + else if (!strcmp(verb, "reset")) { ret = barosim::reset(); + } /* * Print driver information. */ - else if (!strcmp(verb, "info")) + else if (!strcmp(verb, "info")) { ret = barosim::info(); + } /* * Perform MSL pressure calibration given an altitude in metres @@ -1180,10 +1143,11 @@ barosim_main(int argc, char *argv[]) long altitude = strtol(argv[2], nullptr, 10); ret = barosim::calibrate(altitude); - } - else { + + } else { barosim::usage(); return 1; } + return ret; } diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 734c2bc286bd..252d1428c967 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file baro_sim.cpp - * - * Simulation interface for barometer - */ +/** + * @file baro_sim.cpp + * + * Simulation interface for barometer + */ /* XXX trim includes */ #include @@ -118,22 +118,26 @@ BARO_SIM::init() int BARO_SIM::dev_read(unsigned offset, void *data, unsigned count) { + /* union _cvt { uint8_t b[4]; uint32_t w; } *cvt = (_cvt *)data; - uint8_t buf[3]; + */ /* read the most recent measurement */ uint8_t cmd = 0; - int ret = transfer(&cmd, 1, &buf[0], 3); + int ret = transfer(&cmd, 1, static_cast(data), count); + + /* if (ret == PX4_OK) { - /* fetch the raw value */ + // fetch the raw value cvt->b[0] = buf[2]; cvt->b[1] = buf[1]; cvt->b[2] = buf[0]; cvt->b[3] = 0; } + */ return ret; } @@ -178,7 +182,7 @@ int BARO_SIM::_measure(unsigned addr) { /* - * Disable retries on this command; we can't know whether failure + * Disable retries on this command; we can't know whether failure * means the device did or did not see the command. */ _retries = 0; @@ -197,25 +201,28 @@ BARO_SIM::_read_prom() int BARO_SIM::transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len) + uint8_t *recv, unsigned recv_len) { // TODO add Simulation data connection so calls retrieve // data from the simulator if (recv_len == 0) { PX4_DEBUG("BARO_SIM measurement requested"); - } - else if (send_len != 1 || send[0] != 0 || recv_len != 3) { + + } else if (send_len != 1 || send[0] != 0) { PX4_WARN("BARO_SIM::transfer invalid param %u %u %u", send_len, send[0], recv_len); return 1; - } - else { + + } else { Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { PX4_ERR("Error BARO_SIM::transfer no simulator"); return -ENODEV; } + PX4_DEBUG("BARO_SIM::transfer getting sample"); sim->getBaroSample(recv, recv_len); } + return 0; } diff --git a/src/platforms/posix/drivers/barosim/barosim.h b/src/platforms/posix/drivers/barosim/barosim.h index 8eaa0c21f582..bac176d400ed 100644 --- a/src/platforms/posix/drivers/barosim/barosim.h +++ b/src/platforms/posix/drivers/barosim/barosim.h @@ -80,5 +80,5 @@ extern bool crc4(uint16_t *n_prom); } /* namespace */ /* interface factories */ -extern device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); +extern device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum); typedef device::Device *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum); diff --git a/src/platforms/posix/drivers/gpssim/CMakeLists.txt b/src/platforms/posix/drivers/gpssim/CMakeLists.txt new file mode 100644 index 000000000000..2c580d7757c9 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__drivers__gpssim + MAIN gpssim + COMPILE_FLAGS + -Os + SRCS + gpssim.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp new file mode 100644 index 000000000000..889d79348692 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -0,0 +1,605 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 gpssim.cpp + * Simulated GPS driver + */ + +#include +#define __STDC_FORMAT_MACROS +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#define GPS_DRIVER_MODE_UBX_SIM +#define GPSSIM_DEVICE_PATH "/dev/gpssim" + +#define TIMEOUT_5HZ 500 +#define RATE_MEASUREMENT_PERIOD 5000000 + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* class for dynamic allocation of satellite info data */ +class GPS_Sat_Info +{ +public: + struct satellite_info_s _data; +}; + + +class GPSSIM : public device::VDev +{ +public: + GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info); + virtual ~GPSSIM(); + + virtual int init(); + + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +private: + + bool _task_should_exit; ///< flag to make the main worker task exit + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; ///< current baudrate + char _port[20]; ///< device / serial port path + volatile int _task; ///< worker task + bool _healthy; ///< flag to signal if the GPS is ok + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _mode_changed; ///< flag that the GPS mode has changed + //gps_driver_mode_t _mode; ///< current mode + GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object + struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position + orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position + struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info + orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info + float _rate; ///< position update rate + bool _fake_gps; ///< fake gps output + + /** + * Try to configure the GPS, handle outgoing communication to the GPS + */ + void config(); + + /** + * Trampoline to the worker task + */ + static void task_main_trampoline(void *arg); + + + /** + * Worker task: main GPS thread that configures the GPS and parses incoming data, always running + */ + void task_main(void); + + /** + * Set the baudrate of the UART to the GPS + */ + int set_baudrate(unsigned baud); + + /** + * Send a reset command to the GPS + */ + void cmd_reset(); + + int receive(int timeout); +}; + + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int gpssim_main(int argc, char *argv[]); + +namespace +{ + +GPSSIM *g_dev = nullptr; + +} + + +GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info) : + VDev("gps", GPSSIM_DEVICE_PATH), + _task_should_exit(false), + //_healthy(false), + //_mode_changed(false), + //_mode(GPS_DRIVER_MODE_UBX), + //_Helper(nullptr), + _Sat_Info(nullptr), + _report_gps_pos_pub(nullptr), + _p_report_sat_info(nullptr), + _report_sat_info_pub(nullptr), + _rate(0.0f), + _fake_gps(fake_gps) +{ + // /* store port name */ + // strncpy(_port, uart_path, sizeof(_port)); + // /* enforce null termination */ + // _port[sizeof(_port) - 1] = '\0'; + + /* we need this potentially before it could be set in task_main */ + g_dev = this; + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + + /* create satellite info data object if requested */ + if (enable_sat_info) { + _Sat_Info = new(GPS_Sat_Info); + _p_report_sat_info = &_Sat_Info->_data; + memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); + } + + _debug_enabled = true; +} + +GPSSIM::~GPSSIM() +{ + /* tell the task we want it to go away */ + _task_should_exit = true; + + /* spin waiting for the task to stop */ + for (unsigned i = 0; (i < 10) && (_task != -1); i++) { + /* give it another 100ms */ + usleep(100000); + } + + /* well, kill it anyway, though this will probably crash */ + if (_task != -1) { + px4_task_delete(_task); + } + + g_dev = nullptr; +} + +int +GPSSIM::init() +{ + int ret = ERROR; + + /* do regular cdev init */ + if (VDev::init() != OK) { + goto out; + } + + /* start the GPS driver worker task */ + _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPSSIM::task_main_trampoline, nullptr); + + if (_task < 0) { + PX4_ERR("task start failed: %d", errno); + return -errno; + } + + ret = OK; +out: + return ret; +} + +int +GPSSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + lock(); + + int ret = OK; + + switch (cmd) { + case SENSORIOCRESET: + cmd_reset(); + break; + + default: + /* give it to parent if no one wants it */ + ret = VDev::ioctl(filp, cmd, arg); + break; + } + + unlock(); + + return ret; +} + +void +GPSSIM::task_main_trampoline(void *arg) +{ + g_dev->task_main(); +} + +int +GPSSIM::receive(int timeout) +{ + Simulator *sim = Simulator::getInstance(); + simulator::RawGPSData gps; + sim->getGPSSample((uint8_t *)&gps, sizeof(gps)); + + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = gps.lat; + _report_gps_pos.lon = gps.lon; + _report_gps_pos.alt = gps.alt; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.eph = (float)gps.eph * 1e-2f; + _report_gps_pos.epv = (float)gps.epv * 1e-2f; + _report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f; + _report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f; + _report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f; + _report_gps_pos.vel_d_m_s = (float)(gps.vd) / 100.0f; + _report_gps_pos.cog_rad = (float)(gps.cog) * 3.1415f / (100.0f * 180.0f); + _report_gps_pos.fix_type = gps.fix_type; + _report_gps_pos.satellites_used = gps.satellites_visible; + + usleep(200000); + return 1; +} + +void +GPSSIM::task_main() +{ + + /* loop handling received serial bytes and also configuring in between */ + while (!_task_should_exit) { + + if (_fake_gps) { + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = (int32_t)47.378301e7f; + _report_gps_pos.lon = (int32_t)8.538777e7f; + _report_gps_pos.alt = (int32_t)1200e3f; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.s_variance_m_s = 10.0f; + _report_gps_pos.c_variance_rad = 0.1f; + _report_gps_pos.fix_type = 3; + _report_gps_pos.eph = 0.9f; + _report_gps_pos.epv = 1.8f; + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * + _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.vel_ned_valid = true; + + //no time and satellite information simulated + + if (!(_pub_blocked)) { + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + } + + usleep(2e5); + + } else { + //Publish initial report that we have access to a GPS + //Make sure to clear any stale data in case driver is reset + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.timestamp_time = hrt_absolute_time(); + + if (!(_pub_blocked)) { + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + } + + // GPS is obviously detected successfully, reset statistics + //_Helper->reset_update_rates(); + + while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { + /* opportunistic publishing - else invalid data would end up on the bus */ + + if (!(_pub_blocked)) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + if (_p_report_sat_info) { + if (_report_sat_info_pub != nullptr) { + orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); + + } else { + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); + } + } + } + } + + lock(); + } + } + + PX4_INFO("exiting"); + + /* tell the dtor that we are exiting */ + _task = -1; + return; +} + + + +void +GPSSIM::cmd_reset() +{ +} + +void +GPSSIM::print_info() +{ + //GPS Mode + if (_fake_gps) { + PX4_INFO("protocol: faked"); + } + + else { + PX4_INFO("protocol: SIM"); + } + + PX4_INFO("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + PX4_INFO("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + + if (_report_gps_pos.timestamp_position != 0) { + PX4_INFO("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + PX4_INFO("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + PX4_INFO("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); + PX4_INFO("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); + //PX4_INFO("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); + //PX4_INFO("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); + PX4_INFO("rate publication:\t%6.2f Hz", (double)_rate); + + } + + usleep(100000); +} + +/** + * Local functions in support of the shell command. + */ +namespace gpssim +{ + +GPSSIM *g_dev = nullptr; + +void start(const char *uart_path, bool fake_gps, bool enable_sat_info); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start(const char *uart_path, bool fake_gps, bool enable_sat_info) +{ + int fd; + + /* create the driver */ + g_dev = new GPSSIM(uart_path, fake_gps, enable_sat_info); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = px4_open(GPSSIM_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + PX4_ERR("open: %s\n", GPS0_DEVICE_PATH); + goto fail; + } + + return; + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + PX4_ERR("start failed"); +} + +/** + * Stop the driver. + */ +void +stop() +{ + delete g_dev; + g_dev = nullptr; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + PX4_INFO("PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = px4_open(GPSSIM_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + PX4_ERR("failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("reset failed"); + } +} + +/** + * Print the status of the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + PX4_ERR("gpssim not running"); + } + + g_dev->print_info(); +} + +} // namespace + + +int +gpssim_main(int argc, char *argv[]) +{ + + /* set to default */ + const char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; + bool enable_sat_info = false; + + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + if (g_dev != nullptr) { + PX4_WARN("gpssim already started"); + return 0; + } + + /* work around getopt unreliability */ + if (argc > 3) { + if (!strcmp(argv[2], "-d")) { + device_name = argv[3]; + + } else { + goto out; + } + } + + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) { + fake_gps = true; + } + } + + /* Detect sat info option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-s")) { + enable_sat_info = true; + } + } + + gpssim::start(device_name, fake_gps, enable_sat_info); + } + + if (!strcmp(argv[1], "stop")) { + gpssim::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + gpssim::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + gpssim::reset(); + } + + /* + * Print driver status. + */ + if (!strcmp(argv[1], "status")) { + gpssim::info(); + } + + return 0; + +out: + PX4_INFO("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); + return 1; +} diff --git a/src/platforms/posix/drivers/gpssim/module.mk b/src/platforms/posix/drivers/gpssim/module.mk new file mode 100644 index 000000000000..8db69770fe55 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Simulated GPS driver +# + +MODULE_COMMAND = gpssim + +SRCS = gpssim.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/drivers/gpssim/ubx_sim.cpp b/src/platforms/posix/drivers/gpssim/ubx_sim.cpp new file mode 100644 index 000000000000..0e4270402e7c --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/ubx_sim.cpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 ubx.cpp + * + * U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description + * including Prototol Specification. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * + * @author Hannes Delago + * (rework, add ubx7+ compatibility) + * + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "ubx_sim.h" +#include + +#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK +#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received +#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls +#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval + + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) : + _fd(fd), + _gps_position(gps_position), + _satellite_info(satellite_info), +{ +} + +UBX::~UBX() +{ +} + +int UBX_SIM::configure(unsigned &baudrate) +{ + return 0; +} + +int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled +UBX_SIM::receive(const unsigned timeout) +{ + /* copy data from simulator here */ + usleep(1000000); + return 1; +} diff --git a/src/platforms/posix/drivers/gpssim/ubx_sim.h b/src/platforms/posix/drivers/gpssim/ubx_sim.h new file mode 100644 index 000000000000..98f6fdf3bce5 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/ubx_sim.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ubx_sim.h + * + */ + +#ifndef UBX_SIM_H_ +#define UBX_SIM_H_ + +class UBX_SIM +{ +public: + UBX_SIM(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); + ~UBX_SIM(); + int receive(const unsigned timeout); + int configure(unsigned &baudrate); + +private: + int _fd; + struct vehicle_gps_position_s *_gps_position; + struct satellite_info_s *_satellite_info; + bool _enable_sat_info; +}; + +#endif /* UBX_SIM_H_ */ diff --git a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt new file mode 100644 index 000000000000..335bee65d64b --- /dev/null +++ b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__drivers__gyrosim + MAIN gyrosim + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + gyrosim.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 3b59673135f3..c7f78dbca6f5 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -34,13 +34,16 @@ /** * @file gyrosim.cpp * - * Driver for the simulated gyro + * Driver for the simulated gyro * * @author Andrew Tridgell * @author Pat Hickey * @author Mark Charlebois */ +#define __STDC_FORMAT_MACROS +#include + #include #include @@ -49,7 +52,6 @@ #include #include #include -#include #include #include #include @@ -81,97 +83,23 @@ #define MPU_DEVICE_PATH_GYRO "/dev/gyrosim_gyro" // MPU 6000 registers -#define MPUREG_WHOAMI 0x75 #define MPUREG_SMPLRT_DIV 0x19 #define MPUREG_CONFIG 0x1A #define MPUREG_GYRO_CONFIG 0x1B #define MPUREG_ACCEL_CONFIG 0x1C -#define MPUREG_FIFO_EN 0x23 -#define MPUREG_INT_PIN_CFG 0x37 -#define MPUREG_INT_ENABLE 0x38 #define MPUREG_INT_STATUS 0x3A -#define MPUREG_ACCEL_XOUT_H 0x3B -#define MPUREG_ACCEL_XOUT_L 0x3C -#define MPUREG_ACCEL_YOUT_H 0x3D -#define MPUREG_ACCEL_YOUT_L 0x3E -#define MPUREG_ACCEL_ZOUT_H 0x3F -#define MPUREG_ACCEL_ZOUT_L 0x40 -#define MPUREG_TEMP_OUT_H 0x41 -#define MPUREG_TEMP_OUT_L 0x42 -#define MPUREG_GYRO_XOUT_H 0x43 -#define MPUREG_GYRO_XOUT_L 0x44 -#define MPUREG_GYRO_YOUT_H 0x45 -#define MPUREG_GYRO_YOUT_L 0x46 -#define MPUREG_GYRO_ZOUT_H 0x47 -#define MPUREG_GYRO_ZOUT_L 0x48 -#define MPUREG_USER_CTRL 0x6A -#define MPUREG_PWR_MGMT_1 0x6B -#define MPUREG_PWR_MGMT_2 0x6C -#define MPUREG_FIFO_COUNTH 0x72 -#define MPUREG_FIFO_COUNTL 0x73 -#define MPUREG_FIFO_R_W 0x74 #define MPUREG_PRODUCT_ID 0x0C -#define MPUREG_TRIM1 0x0D -#define MPUREG_TRIM2 0x0E -#define MPUREG_TRIM3 0x0F -#define MPUREG_TRIM4 0x10 - -// Configuration bits MPU 3000 and MPU 6000 (not revised)? -#define BIT_SLEEP 0x40 -#define BIT_H_RESET 0x80 -#define BITS_CLKSEL 0x07 -#define MPU_CLK_SEL_PLLGYROX 0x01 -#define MPU_CLK_SEL_PLLGYROZ 0x03 -#define MPU_EXT_SYNC_GYROX 0x02 -#define BITS_GYRO_ST_X 0x80 -#define BITS_GYRO_ST_Y 0x40 -#define BITS_GYRO_ST_Z 0x20 -#define BITS_FS_250DPS 0x00 -#define BITS_FS_500DPS 0x08 -#define BITS_FS_1000DPS 0x10 -#define BITS_FS_2000DPS 0x18 -#define BITS_FS_MASK 0x18 -#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 -#define BITS_DLPF_CFG_188HZ 0x01 -#define BITS_DLPF_CFG_98HZ 0x02 -#define BITS_DLPF_CFG_42HZ 0x03 -#define BITS_DLPF_CFG_20HZ 0x04 -#define BITS_DLPF_CFG_10HZ 0x05 -#define BITS_DLPF_CFG_5HZ 0x06 -#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 -#define BITS_DLPF_CFG_MASK 0x07 -#define BIT_INT_ANYRD_2CLEAR 0x10 -#define BIT_RAW_RDY_EN 0x01 -#define BIT_I2C_IF_DIS 0x10 -#define BIT_INT_STATUS_DATA 0x01 // Product ID Description for GYROSIM // high 4 bits low 4 bits // Product Name Product Revision #define GYROSIMES_REV_C4 0x14 -#define GYROSIMES_REV_C5 0x15 -#define GYROSIMES_REV_D6 0x16 -#define GYROSIMES_REV_D7 0x17 -#define GYROSIMES_REV_D8 0x18 -#define GYROSIM_REV_C4 0x54 -#define GYROSIM_REV_C5 0x55 -#define GYROSIM_REV_D6 0x56 -#define GYROSIM_REV_D7 0x57 -#define GYROSIM_REV_D8 0x58 -#define GYROSIM_REV_D9 0x59 -#define GYROSIM_REV_D10 0x5A - -#define GYROSIM_ACCEL_DEFAULT_RANGE_G 8 -#define GYROSIM_ACCEL_DEFAULT_RATE 1000 -#define GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 - -#define GYROSIM_GYRO_DEFAULT_RANGE_G 8 -#define GYROSIM_GYRO_DEFAULT_RATE 1000 -#define GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 - -#define GYROSIM_DEFAULT_ONCHIP_FILTER_FREQ 42 - -#define GYROSIM_ONE_G 9.80665f + +#define GYROSIM_ACCEL_DEFAULT_RATE 400 + +#define GYROSIM_GYRO_DEFAULT_RATE 400 + +#define GYROSIM_ONE_G 9.80665f #ifdef PX4_SPI_BUS_EXT #define EXTERNAL_BUS PX4_SPI_BUS_EXT @@ -184,9 +112,6 @@ interrupt status registers. All other registers have a maximum 1MHz SPI speed */ -#define GYROSIM_LOW_BUS_SPEED 1000*1000 -#define GYROSIM_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */ - class GYROSIM_gyro; class GYROSIM : public device::VDev @@ -240,33 +165,13 @@ class GYROSIM : public device::VDev perf_counter_t _accel_reads; perf_counter_t _gyro_reads; perf_counter_t _sample_perf; - perf_counter_t _bad_transfers; - perf_counter_t _bad_registers; perf_counter_t _good_transfers; perf_counter_t _reset_retries; perf_counter_t _system_latency_perf; perf_counter_t _controller_latency_perf; - uint8_t _register_wait; - uint64_t _reset_wait; - - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; - math::LowPassFilter2p _gyro_filter_x; - math::LowPassFilter2p _gyro_filter_y; - math::LowPassFilter2p _gyro_filter_z; - enum Rotation _rotation; - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset -#define GYROSIM_NUM_CHECKED_REGISTERS 9 - static const uint8_t _checked_registers[GYROSIM_NUM_CHECKED_REGISTERS]; - uint8_t _checked_values[GYROSIM_NUM_CHECKED_REGISTERS]; - uint8_t _checked_next; - // last temperature reading for print_info() float _last_temperature; @@ -309,8 +214,7 @@ class GYROSIM : public device::VDev * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg, uint32_t speed=GYROSIM_LOW_BUS_SPEED); - uint16_t read_reg16(unsigned reg); + uint8_t read_reg(unsigned reg); /** * Write a register in the GYROSIM @@ -320,25 +224,6 @@ class GYROSIM : public device::VDev */ void write_reg(unsigned reg, uint8_t value); - /** - * Modify a register in the GYROSIM - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Write a register in the GYROSIM, updating _checked_values - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_checked_reg(unsigned reg, uint8_t value); - /** * Set the GYROSIM measurement range. * @@ -357,7 +242,7 @@ class GYROSIM : public device::VDev * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /** * Accel self test @@ -371,26 +256,16 @@ class GYROSIM : public device::VDev * * @return 0 on success, 1 on failure */ - int gyro_self_test(); - - /* - set low pass filter frequency - */ - void _set_dlpf_filter(uint16_t frequency_hz); + int gyro_self_test(); /* set sample rate (approximate) - 1kHz to 5Hz */ void _set_sample_rate(unsigned desired_sample_rate_hz); - /* - check that key registers still have the right value - */ - void check_registers(void); - /* do not allow to copy this class due to pointer data members */ - GYROSIM(const GYROSIM&); - GYROSIM operator=(const GYROSIM&); + GYROSIM(const GYROSIM &); + GYROSIM operator=(const GYROSIM &); #pragma pack(push, 1) /** @@ -400,35 +275,19 @@ class GYROSIM : public device::VDev struct MPUReport { uint8_t cmd; uint8_t status; - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; + float accel_x; + float accel_y; + float accel_z; + float temp; + float gyro_x; + float gyro_y; + float gyro_z; }; #pragma pack(pop) uint8_t _regdata[108]; }; -/* - list of registers that will be checked in check_registers(). Note - that MPUREG_PRODUCT_ID must be first in the list. - */ -const uint8_t GYROSIM::_checked_registers[GYROSIM_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, - MPUREG_PWR_MGMT_1, - MPUREG_USER_CTRL, - MPUREG_SMPLRT_DIV, - MPUREG_CONFIG, - MPUREG_GYRO_CONFIG, - MPUREG_ACCEL_CONFIG, - MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG }; - - - /** * Helper class implementing the gyro driver node. */ @@ -455,8 +314,8 @@ class GYROSIM_gyro : public device::VDev int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ - GYROSIM_gyro(const GYROSIM_gyro&); - GYROSIM_gyro operator=(const GYROSIM_gyro&); + GYROSIM_gyro(const GYROSIM_gyro &); + GYROSIM_gyro operator=(const GYROSIM_gyro &); }; /** driver 'main' command */ @@ -483,27 +342,19 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _accel_reads(perf_alloc(PC_COUNT, "gyrosim_accel_read")), _gyro_reads(perf_alloc(PC_COUNT, "gyrosim_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "gyrosim_read")), - _bad_transfers(perf_alloc(PC_COUNT, "gyrosim_bad_transfers")), - _bad_registers(perf_alloc(PC_COUNT, "gyrosim_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "gyrosim_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "gyrosim_reset_retries")), _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), - _register_wait(0), - _reset_wait(0), - _accel_filter_x(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_y(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_z(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_x(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_y(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_z(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), - _checked_next(0), _last_temperature(0) { // disable debug() calls _debug_enabled = false; + // Don't publish until initialized + _pub_blocked = true; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; /* Prime _gyro with parents devid. */ @@ -527,7 +378,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _gyro_scale.z_scale = 1.0f; memset(&_call, 0, sizeof(_call)); - _checked_values[0] = _product; } GYROSIM::~GYROSIM() @@ -539,20 +389,22 @@ GYROSIM::~GYROSIM() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_gyro_reports != nullptr) + } + + if (_gyro_reports != nullptr) { delete _gyro_reports; + } - if (_accel_class_instance != -1) + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); perf_free(_accel_reads); perf_free(_gyro_reads); - perf_free(_bad_transfers); - perf_free(_bad_registers); perf_free(_good_transfers); } @@ -570,14 +422,20 @@ GYROSIM::init() return ret; } + struct accel_report arp = {}; + + struct gyro_report grp = {}; + /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + if (_accel_reports == nullptr) { PX4_WARN("_accel_reports creation failed"); goto out; } _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + if (_gyro_reports == nullptr) { PX4_WARN("_gyro_reports creation failed"); goto out; @@ -606,9 +464,10 @@ GYROSIM::init() /* do VDev init for the gyro device node, keep it optional */ ret = _gyro->init(); + /* if probe/setup failed, bail now */ if (ret != OK) { - debug("gyro init failed"); + DEVICE_DEBUG("gyro init failed"); return ret; } @@ -617,24 +476,25 @@ GYROSIM::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, ORB_PRIO_HIGH); + &_accel_orb_class_instance, ORB_PRIO_HIGH); if (_accel_topic == nullptr) { PX4_WARN("ADVERT FAIL"); + + } else { + _pub_blocked = false; } /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); + &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); if (_gyro->_gyro_topic == nullptr) { PX4_WARN("ADVERT FAIL"); @@ -659,24 +519,29 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) if (cmd == MPUREAD) { // Get data from the simulator Simulator *sim = Simulator::getInstance(); - if (sim == NULL) + + if (sim == NULL) { + PX4_WARN("failed accessing simulator"); return ENODEV; + } // FIXME - not sure what interrupt status should be recv[1] = 0; // skip cmd and status bytes - sim->getMPUReport(&recv[2], len-2); - } - else if (cmd & DIR_READ) - { - PX4_DEBUG("Reading %u bytes from register %u", len-1, reg); - memcpy(&_regdata[reg-MPUREG_PRODUCT_ID], &send[1], len-1); - } - else { - PX4_DEBUG("Writing %u bytes to register %u", len-1, reg); - if (recv) - memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1); + sim->getMPUReport(&recv[2], len - 2); + + } else if (cmd & DIR_READ) { + PX4_DEBUG("Reading %u bytes from register %u", len - 1, reg); + memcpy(&_regdata[reg - MPUREG_PRODUCT_ID], &send[1], len - 1); + + } else { + PX4_DEBUG("Writing %u bytes to register %u", len - 1, reg); + + if (recv) { + memcpy(&recv[1], &_regdata[reg - MPUREG_PRODUCT_ID], len - 1); + } } + return PX4_OK; } @@ -686,50 +551,29 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) void GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) { + PX4_INFO("GYROSIM::_set_sample_rate %uHz", desired_sample_rate_hz); + if (desired_sample_rate_hz == 0 || - desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || - desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { desired_sample_rate_hz = GYROSIM_GYRO_DEFAULT_RATE; } uint8_t div = 1000 / desired_sample_rate_hz; - if(div>200) div=200; - if(div<1) div=1; - write_checked_reg(MPUREG_SMPLRT_DIV, div-1); - _sample_rate = 1000 / div; -} -/* - set the DLPF filter frequency. This affects both accel and gyro. - */ -void -GYROSIM::_set_dlpf_filter(uint16_t frequency_hz) -{ - uint8_t filter; + if (div > 200) { div = 200; } - /* - choose next highest filter frequency available - */ - if (frequency_hz == 0) { - filter = BITS_DLPF_CFG_2100HZ_NOLPF; - } else if (frequency_hz <= 5) { - filter = BITS_DLPF_CFG_5HZ; - } else if (frequency_hz <= 10) { - filter = BITS_DLPF_CFG_10HZ; - } else if (frequency_hz <= 20) { - filter = BITS_DLPF_CFG_20HZ; - } else if (frequency_hz <= 42) { - filter = BITS_DLPF_CFG_42HZ; - } else if (frequency_hz <= 98) { - filter = BITS_DLPF_CFG_98HZ; - } else if (frequency_hz <= 188) { - filter = BITS_DLPF_CFG_188HZ; - } else if (frequency_hz <= 256) { - filter = BITS_DLPF_CFG_256HZ_NOLPF2; - } else { - filter = BITS_DLPF_CFG_2100HZ_NOLPF; - } - write_checked_reg(MPUREG_CONFIG, filter); + if (div < 1) { div = 1; } + + // This does nothing in the simulator but writes the value in the "register" so + // register dumps look correct + write_reg(MPUREG_SMPLRT_DIV, div - 1); + + _sample_rate = 1000 / div; + PX4_INFO("GYROSIM: Changed sample rate to %uHz", _sample_rate); + _call_interval = 1000000 / _sample_rate; + hrt_cancel(&_call); + hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); } ssize_t @@ -738,8 +582,9 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -748,17 +593,21 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) + if (_accel_reports->empty()) { return -EAGAIN; + } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_accel_reports->get(arp)) + if (!_accel_reports->get(arp)) { break; + } + transferred++; arp++; } @@ -783,24 +632,34 @@ GYROSIM::accel_self_test() { return OK; - if (self_test()) + if (self_test()) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.z_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { return 1; + } return 0; } @@ -810,8 +669,9 @@ GYROSIM::gyro_self_test() { return OK; - if (self_test()) + if (self_test()) { return 1; + } /* * Maximum deviation of 20 degrees, according to @@ -827,27 +687,35 @@ GYROSIM::gyro_self_test() 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) + if (fabsf(_gyro_scale.x_offset) > max_offset) { return 1; + } /* evaluate gyro scale, complain if off by more than 30% */ - if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > max_offset) + if (fabsf(_gyro_scale.y_offset) > max_offset) { return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + } + + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > max_offset) + if (fabsf(_gyro_scale.z_offset) > max_offset) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + } + + 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)) { + (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; } @@ -861,8 +729,9 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -871,17 +740,21 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) + if (_gyro_reports->empty()) { return -EAGAIN; + } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_gyro_reports->get(grp)) + if (!_gyro_reports->get(grp)) { break; + } + transferred++; grp++; } @@ -901,60 +774,46 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); - /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; - - // adjust filters - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; - _set_dlpf_filter(cutoff_freq_hz); - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - - - float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); - _set_dlpf_filter(cutoff_freq_hz_gyro); - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + } /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; + _call_interval = ticks; + + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -962,22 +821,24 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; 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; + } - if (!_accel_reports->resize(arg)) { - return -ENOMEM; - } + if (!_accel_reports->resize(arg)) { + return -ENOMEM; + } - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -989,26 +850,18 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) _set_sample_rate(arg); return OK; - case ACCELIOCGLOWPASS: - return _accel_filter_x.get_cutoff_freq(); - case ACCELIOCSLOWPASS: - // set hardware filtering - _set_dlpf_filter(arg); - // set software filtering - _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; - case ACCELIOCSSCALE: - { + case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; + } else { return -EINVAL; } @@ -1023,7 +876,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return set_accel_range(arg); case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/GYROSIM_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / GYROSIM_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -1039,23 +892,24 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - /* these are shared with the accel side */ + /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); 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; + } - if (!_gyro_reports->resize(arg)) { - return -ENOMEM; - } + if (!_gyro_reports->resize(arg)) { + return -ENOMEM; + } - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -1067,14 +921,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) _set_sample_rate(arg); return OK; - case GYROIOCGLOWPASS: - return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSLOWPASS: - // set hardware filtering - _set_dlpf_filter(arg); - _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case GYROIOCSSCALE: @@ -1093,6 +940,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) // _gyro_range_scale = xx // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); @@ -1106,31 +954,16 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) } uint8_t -GYROSIM::read_reg(unsigned reg, uint32_t speed) +GYROSIM::read_reg(unsigned reg) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; - // general register transfer at low clock speed - //set_frequency(speed); - + // general register transfer at low clock speed transfer(cmd, cmd, sizeof(cmd)); return cmd[1]; } -uint16_t -GYROSIM::read_reg16(unsigned reg) -{ - uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; - - // general register transfer at low clock speed - //set_frequency(GYROSIM_LOW_BUS_SPEED); - - transfer(cmd, cmd, sizeof(cmd)); - - return (uint16_t)(cmd[1] << 8) | cmd[2]; -} - void GYROSIM::write_reg(unsigned reg, uint8_t value) { @@ -1139,47 +972,20 @@ GYROSIM::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - // general register transfer at low clock speed - //set_frequency(GYROSIM_LOW_BUS_SPEED); - + // general register transfer at low clock speed transfer(cmd, nullptr, sizeof(cmd)); } -void -GYROSIM::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val; - - val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_reg(reg, val); -} - -void -GYROSIM::write_checked_reg(unsigned reg, uint8_t value) -{ - write_reg(reg, value); - for (uint8_t i=0; i 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; max_accel_g = 2; } - write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); + write_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); _accel_range_scale = (GYROSIM_ONE_G / lsb_per_g); _accel_range_m_s2 = max_accel_g * GYROSIM_ONE_G; @@ -1222,7 +1031,9 @@ GYROSIM::start() _gyro_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); + if (_call_interval > 0) { + hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); + } } void @@ -1240,76 +1051,24 @@ GYROSIM::measure_trampoline(void *arg) dev->measure(); } -void -GYROSIM::check_registers(void) -{ - /* - we read the register at full speed, even though it isn't - listed as a high speed register. The low speed requirement - for some registers seems to be a propgation delay - requirement for changing sensor configuration, which should - not apply to reading a single register. It is also a better - test of VDev bus health to read at the same speed as we read - the data registers. - */ - uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], GYROSIM_HIGH_BUS_SPEED)) != - _checked_values[_checked_next]) { - /* - if we get the wrong value then we know the VDev bus - or sensor is very sick. We set _register_wait to 20 - and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. - */ - perf_count(_bad_registers); - - /* - try to fix the bad register value. We only try to - fix one per loop to prevent a bad sensor hogging the - bus. - */ - if (_register_wait == 0 || _checked_next == 0) { - // if the product_id is wrong then reset the - // sensor completely - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - // after doing a reset we need to wait a long - // time before we do any other register writes - // or we will end up with the gyrosim in a - // bizarre state where it has all correct - // register values but large offsets on the - // accel axes - _reset_wait = hrt_absolute_time() + 10000; - _checked_next = 0; - } else { - write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); - // waiting 3ms between register writes seems - // to raise the chance of the sensor - // recovering considerably - _reset_wait = hrt_absolute_time() + 3000; - } - _register_wait = 20; - } - _checked_next = (_checked_next+1) % GYROSIM_NUM_CHECKED_REGISTERS; -} - void GYROSIM::measure() { - if (hrt_absolute_time() < _reset_wait) { - // we're waiting for a reset to complete - return; + +#if 0 + static int x = 0; + + // Verify the samples are being taken at the expected rate + if (x == 99) { + x = 0; + PX4_INFO("GYROSIM::measure %" PRIu64, hrt_absolute_time()); + + } else { + x++; } - struct MPUReport mpu_report; - struct Report { - int16_t accel_x; - int16_t accel_y; - int16_t accel_z; - int16_t temp; - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; - } report; +#endif + struct MPUReport mpu_report = {}; /* start measuring */ perf_begin(_sample_perf); @@ -1319,89 +1078,26 @@ GYROSIM::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - check_registers(); - - // sensor transfer at high clock speed - //set_frequency(GYROSIM_HIGH_BUS_SPEED); - - if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) - return; - - /* - * Convert from big to little endian - */ - - report.accel_x = int16_t_from_bytes(mpu_report.accel_x); - report.accel_y = int16_t_from_bytes(mpu_report.accel_y); - report.accel_z = int16_t_from_bytes(mpu_report.accel_z); - - report.temp = int16_t_from_bytes(mpu_report.temp); - - report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); - report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); - report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); - - if (report.accel_x == 0 && - report.accel_y == 0 && - report.accel_z == 0 && - report.temp == 0 && - report.gyro_x == 0 && - report.gyro_y == 0 && - report.gyro_z == 0) { - // all zero data - probably a VDev bus error - perf_count(_bad_transfers); - perf_end(_sample_perf); - // note that we don't call reset() here as a reset() - // costs 20ms with interrupts disabled. That means if - // the mpu6k does go bad it would cause a FMU failure, - // regardless of whether another sensor is available, - return; - } - - perf_count(_good_transfers); - - if (_register_wait != 0) { - // we are waiting for some good transfers before using - // the sensor again. We still increment - // _good_transfers, but don't return any data yet - _register_wait--; + // sensor transfer at high clock speed + //set_frequency(GYROSIM_HIGH_BUS_SPEED); + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { return; } - - /* - * Swap axes and negate y - */ - int16_t accel_xt = report.accel_y; - int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); - - int16_t gyro_xt = report.gyro_y; - int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); - - /* - * Apply the swap - */ - report.accel_x = accel_xt; - report.accel_y = accel_yt; - report.gyro_x = gyro_xt; - report.gyro_y = gyro_yt; - /* * Report buffers. */ - accel_report arb; - gyro_report grb; - - /* - * Adjust and scale results to m/s^2. - */ - grb.timestamp = arb.timestamp = hrt_absolute_time(); + accel_report arb = {}; + gyro_report grb = {}; + // for now use local time but this should be the timestamp of the simulator + grb.timestamp = hrt_absolute_time(); + arb.timestamp = grb.timestamp; // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + grb.error_count = arb.error_count = 0; // FIXME /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1421,61 +1117,40 @@ GYROSIM::measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - arb.x_raw = report.accel_x; - arb.y_raw = report.accel_y; - arb.z_raw = report.accel_z; - - float xraw_f = report.accel_x; - float yraw_f = report.accel_y; - float zraw_f = report.accel_z; - - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - - arb.x = _accel_filter_x.apply(x_in_new); - arb.y = _accel_filter_y.apply(y_in_new); - arb.z = _accel_filter_z.apply(z_in_new); + arb.x_raw = (int16_t)(mpu_report.accel_x / _accel_range_scale); + arb.y_raw = (int16_t)(mpu_report.accel_y / _accel_range_scale); + arb.z_raw = (int16_t)(mpu_report.accel_z / _accel_range_scale); arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; - _last_temperature = (report.temp) / 361.0f + 35.0f; + _last_temperature = mpu_report.temp; - arb.temperature_raw = report.temp; + arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f) * 361.0f); arb.temperature = _last_temperature; - grb.x_raw = report.gyro_x; - grb.y_raw = report.gyro_y; - grb.z_raw = report.gyro_z; - - xraw_f = report.gyro_x; - yraw_f = report.gyro_y; - zraw_f = report.gyro_z; + arb.x = mpu_report.accel_x; + arb.y = mpu_report.accel_y; + arb.z = mpu_report.accel_z; - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - - grb.x = _gyro_filter_x.apply(x_gyro_in_new); - grb.y = _gyro_filter_y.apply(y_gyro_in_new); - grb.z = _gyro_filter_z.apply(z_gyro_in_new); + grb.x_raw = (int16_t)(mpu_report.gyro_x / _gyro_range_scale); + grb.y_raw = (int16_t)(mpu_report.gyro_y / _gyro_range_scale); + grb.z_raw = (int16_t)(mpu_report.gyro_z / _gyro_range_scale); grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; - grb.temperature_raw = report.temp; + grb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f) * 361.0f); grb.temperature = _last_temperature; + grb.x = mpu_report.gyro_x; + grb.y = mpu_report.gyro_y; + grb.z = mpu_report.gyro_z; + _accel_reports->force(&arb); _gyro_reports->force(&grb); + /* notify anyone waiting for data */ poll_notify(POLLIN); _gyro->parent_poll_notify(); @@ -1503,44 +1178,35 @@ GYROSIM::print_info() perf_print_counter(_sample_perf); perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); - perf_print_counter(_bad_transfers); - perf_print_counter(_bad_registers); perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); - PX4_WARN("checked_next: %u", _checked_next); - for (uint8_t i=0; igyro_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->gyro_ioctl(filp, cmd, arg); } } @@ -1629,7 +1297,7 @@ int start(enum Rotation rotation) { int fd; - GYROSIM **g_dev_ptr = &g_dev_sim; + GYROSIM **g_dev_ptr = &g_dev_sim; const char *path_accel = MPU_DEVICE_PATH_ACCEL; const char *path_gyro = MPU_DEVICE_PATH_GYRO; @@ -1642,17 +1310,20 @@ start(enum Rotation rotation) /* create the driver */ *g_dev_ptr = new GYROSIM(path_accel, path_gyro, rotation); - if (*g_dev_ptr == nullptr) + if (*g_dev_ptr == nullptr) { goto fail; + } - if (OK != (*g_dev_ptr)->init()) + if (OK != (*g_dev_ptr)->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = px4_open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { px4_close(fd); @@ -1664,8 +1335,8 @@ start(enum Rotation rotation) fail: if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; } PX4_WARN("driver start failed"); @@ -1676,13 +1347,16 @@ int stop() { GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr != nullptr) { delete *g_dev_ptr; *g_dev_ptr = nullptr; + } else { /* warn, but not an error */ PX4_WARN("already stopped."); } + return 0; } @@ -1723,7 +1397,7 @@ test() } /* do a simple demand read */ - sz = read(fd, &a_report, sizeof(a_report)); + sz = px4_read(fd, &a_report, sizeof(a_report)); if (sz != sizeof(a_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(a_report)); @@ -1740,10 +1414,10 @@ test() PX4_INFO("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); PX4_INFO("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); PX4_INFO("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); + (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); /* do a simple demand read */ - sz = read(fd_gyro, &g_report, sizeof(g_report)); + sz = px4_read(fd_gyro, &g_report, sizeof(g_report)); if (sz != sizeof(g_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(g_report)); @@ -1758,7 +1432,7 @@ test() PX4_INFO("gyro y: \t%d\traw", (int)g_report.y_raw); PX4_INFO("gyro z: \t%d\traw", (int)g_report.z_raw); PX4_INFO("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); PX4_INFO("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); PX4_INFO("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); @@ -1770,7 +1444,7 @@ test() reset(); PX4_INFO("PASS"); - + return 0; } @@ -1799,11 +1473,11 @@ reset() goto reset_fail; } - px4_close(fd); + px4_close(fd); return 0; reset_fail: - px4_close(fd); + px4_close(fd); return 1; } @@ -1813,7 +1487,8 @@ reset() int info() { - GYROSIM **g_dev_ptr = &g_dev_sim; + GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { PX4_ERR("driver not running"); return 1; @@ -1832,6 +1507,7 @@ int regdump() { GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { PX4_ERR("driver not running"); return 1; @@ -1846,9 +1522,9 @@ regdump() void usage() { - PX4_WARN("missing command: try 'start', 'info', 'test', 'stop', 'reset', 'regdump'"); - PX4_WARN("options:"); - PX4_WARN(" -R rotation"); + PX4_INFO("missing command: try 'start', 'info', 'test', 'stop', 'reset', 'regdump'"); + PX4_INFO("options:"); + PX4_INFO(" -R rotation"); } } // namespace @@ -1863,11 +1539,13 @@ gyrosim_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'R': - rotation = (enum Rotation)atoi(optarg); + rotation = (enum Rotation)atoi(myoptarg); break; + default: gyrosim::usage(); return 0; diff --git a/src/platforms/posix/drivers/tonealrmsim/CMakeLists.txt b/src/platforms/posix/drivers/tonealrmsim/CMakeLists.txt new file mode 100644 index 000000000000..0ac134ea9c7a --- /dev/null +++ b/src/platforms/posix/drivers/tonealrmsim/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__drivers__tonealrmsim + MAIN tone_alarm + COMPILE_FLAGS + -Os + SRCS + tone_alarm.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index 37f51ff6076a..b58cccb5bd59 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -47,16 +47,16 @@ * From Wikibooks: * * PLAY "[string expression]" - * + * * Used to play notes and a score ... The tones are indicated by letters A through G. - * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) * immediately after the note letter. See this example: - * + * * PLAY "C C# C C#" * * Whitespaces are ignored inside the string expression. There are also codes that - * set the duration, octave and tempo. They are all case-insensitive. PLAY executes - * the commands or notes the order in which they appear in the string. Any indicators + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators * that change the properties are effective for the notes following that indicator. * * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration @@ -66,15 +66,15 @@ * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. - * Remember that C- is equivalent to B. + * Remember that C- is equivalent to B. * < > Changes the current octave respectively down or up one level. * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. - * Pn Causes a silence (pause) for the length of note indicated (same as Ln). - * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. * It can be used for a pause as well. @@ -120,14 +120,15 @@ class ToneAlarm : public device::VDev virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual ssize_t write(device::file_t *filp, const char *buffer, size_t len); - inline const char *name(int tune) { + inline const char *name(int tune) + { return _tune_names[tune]; } private: static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes - const char * _default_tunes[TONE_NUMBER_OF_TUNES]; - const char * _tune_names[TONE_NUMBER_OF_TUNES]; + const char *_default_tunes[TONE_NUMBER_OF_TUNES]; + const char *_tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -151,8 +152,8 @@ class ToneAlarm : public device::VDev // unsigned note_to_divisor(unsigned note); - // Calculate the duration in microseconds of play and silence for a - // note given the current tempo, length and mode and the number of + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of // dots following in the play string. // unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); @@ -257,17 +258,18 @@ ToneAlarm::init() ret = VDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } - debug("ready"); + DEVICE_DEBUG("ready"); return OK; } unsigned ToneAlarm::note_to_divisor(unsigned note) { - const int TONE_ALARM_CLOCK = 120000000ul/4; + const int TONE_ALARM_CLOCK = 120000000ul / 4; // compute the frequency first (Hz) float freq = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f); @@ -285,25 +287,31 @@ ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (note_length == 0) + if (note_length == 0) { note_length = 1; + } + unsigned note_period = whole_note_period / note_length; switch (_note_mode) { case MODE_NORMAL: silence = note_period / 8; break; + case MODE_STACCATO: silence = note_period / 4; break; + default: case MODE_LEGATO: silence = 0; break; } + note_period -= silence; unsigned dot_extension = note_period / 2; + while (dots--) { note_period += dot_extension; dot_extension /= 2; @@ -317,12 +325,14 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (rest_length == 0) + if (rest_length == 0) { rest_length = 1; + } unsigned rest_period = whole_note_period / rest_length; unsigned dot_extension = rest_period / 2; + while (dots--) { rest_period += dot_extension; dot_extension /= 2; @@ -350,7 +360,7 @@ ToneAlarm::start_note(unsigned note) // Silence warning of unused var do_something(period); - PX4_DEBUG("ToneAlarm::start_note %u", period); + PX4_INFO("ToneAlarm::start_note %u", period); } void @@ -361,6 +371,7 @@ ToneAlarm::stop_note() void ToneAlarm::start_tune(const char *tune) { + PX4_INFO("ToneAlarm::start_tune"); // kill any current playback hrt_cancel(&_note_call); @@ -405,115 +416,155 @@ ToneAlarm::next_note() while (note == 0) { // we always need at least one character from the string int c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_end; + } + _next++; switch (c) { case 'L': // select note length _note_length = next_number(); - if (_note_length < 1) + + if (_note_length < 1) { goto tune_error; + } + break; case 'O': // select octave _octave = next_number(); - if (_octave > 6) + + if (_octave > 6) { _octave = 6; + } + break; case '<': // decrease octave - if (_octave > 0) + if (_octave > 0) { _octave--; + } + break; case '>': // increase octave - if (_octave < 6) + if (_octave < 6) { _octave++; + } + break; case 'M': // select inter-note gap c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_error; + } + _next++; + switch (c) { case 'N': _note_mode = MODE_NORMAL; break; + case 'L': _note_mode = MODE_LEGATO; break; + case 'S': _note_mode = MODE_STACCATO; break; + case 'F': _repeat = false; break; + case 'B': _repeat = true; break; + default: goto tune_error; } + break; case 'P': // pause for a note length stop_note(); - hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(next_number(), next_dots()), - (hrt_callout)next_trampoline, - this); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); return; case 'T': { // change tempo - unsigned nt = next_number(); + unsigned nt = next_number(); - if ((nt >= 32) && (nt <= 255)) { - _tempo = nt; - } else { - goto tune_error; + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + + } else { + goto tune_error; + } + + break; } - break; - } case 'N': // play an arbitrary note note = next_number(); - if (note > 84) + + if (note > 84) { goto tune_error; + } + if (note == 0) { // this is a rest - pause for the current note length hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(_note_length, next_dots()), - (hrt_callout)next_trampoline, - this); - return; + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; } + break; case 'A'...'G': // play a note in the current octave note = _note_tab[c - 'A'] + (_octave * 12) + 1; c = next_char(); + switch (c) { case '#': // up a semitone case '+': - if (note < 84) + if (note < 84) { note++; + } + _next++; break; + case '-': // down a semitone - if (note > 1) + if (note > 1) { note--; + } + _next++; break; + default: // 0 / no next char here is OK break; } + // shorthand length notation note_length = next_number(); - if (note_length == 0) + + if (note_length == 0) { note_length = _note_length; + } + break; default: @@ -533,18 +584,21 @@ ToneAlarm::next_note() // tune looks bad (unexpected EOF, bad character, etc.) tune_error: - printf("tune error\n"); + PX4_ERR("tune error\n"); _repeat = false; // don't loop on error // stop (and potentially restart) the tune tune_end: stop_note(); + if (_repeat) { start_tune(_tune); + } else { _tune = nullptr; _default_tune_number = 0; } + return; } @@ -554,6 +608,7 @@ ToneAlarm::next_char() while (isspace(*_next)) { _next++; } + return toupper(*_next); } @@ -565,8 +620,11 @@ ToneAlarm::next_number() for (;;) { c = next_char(); - if (!isdigit(c)) + + if (!isdigit(c)) { return number; + } + _next++; number = (number * 10) + (c - '0'); } @@ -581,6 +639,7 @@ ToneAlarm::next_dots() _next++; dots++; } + return dots; } @@ -598,14 +657,14 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int result = OK; - debug("ioctl %i %u", cmd, arg); + DEVICE_DEBUG("ioctl %i %lu", cmd, arg); // irqstate_t flags = irqsave(); /* decide whether to increase the alarm level to cmd or leave it alone */ switch (cmd) { case TONE_SET_ALARM: - debug("TONE_SET_ALARM %u", arg); + PX4_INFO("TONE_SET_ALARM %lu", arg); if (arg < TONE_NUMBER_OF_TUNES) { if (arg == TONE_STOP_TUNE) { @@ -614,6 +673,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) _next = nullptr; _repeat = false; _default_tune_number = 0; + } else { /* always interrupt alarms, unless they are repeating and already playing */ if (!(_repeat && _default_tune_number == arg)) { @@ -622,6 +682,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) start_tune(_default_tunes[arg]); } } + } else { result = -EINVAL; } @@ -636,8 +697,9 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) // irqrestore(flags); /* give it to the superclass if we didn't like it */ - if (result == -ENOTTY) + if (result == -ENOTTY) { result = VDev::ioctl(filp, cmd, arg); + } return result; } @@ -646,8 +708,9 @@ ssize_t ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) { // sanity-check the buffer for length and nul-termination - if (len > _tune_max) + if (len > _tune_max) { return -EFBIG; + } // if we have an existing user tune, free it if (_user_tune != nullptr) { @@ -664,13 +727,16 @@ ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) } // if the new tune is empty, we're done - if (buffer[0] == '\0') + if (buffer[0] == '\0') { return OK; + } // allocate a copy of the new tune _user_tune = strndup(buffer, len); - if (_user_tune == nullptr) + + if (_user_tune == nullptr) { return -ENOMEM; + } // and play it start_tune(_user_tune); @@ -724,13 +790,15 @@ play_string(const char *str, bool free_buffer) ret = px4_write(fd, str, strlen(str) + 1); px4_close(fd); - if (free_buffer) + if (free_buffer) { free((void *)str); + } if (ret < 0) { PX4_WARN("play tune"); return 1; } + return ret; } @@ -765,31 +833,38 @@ tone_alarm_main(int argc, char *argv[]) ret = play_tune(TONE_STOP_TUNE); } - else if (!strcmp(argv1, "stop")) + else if (!strcmp(argv1, "stop")) { ret = play_tune(TONE_STOP_TUNE); + } - else if ((tune = strtol(argv1, nullptr, 10)) != 0) + else if ((tune = strtol(argv1, nullptr, 10)) != 0) { ret = play_tune(tune); + } /* If it is a file name then load and play it as a string */ else if (*argv1 == '/') { FILE *fd = fopen(argv1, "r"); int sz; char *buffer; + if (fd == nullptr) { PX4_WARN("couldn't open '%s'", argv1); return 1; } + fseek(fd, 0, SEEK_END); sz = ftell(fd); fseek(fd, 0, SEEK_SET); buffer = (char *)malloc(sz + 1); + if (buffer == nullptr) { PX4_WARN("not enough memory memory"); return 1; } + // FIXME - Make GCC happy if (fread(buffer, sz, 1, fd)) { } + /* terminate the string */ buffer[sz] = 0; ret = play_string(buffer, true); @@ -800,14 +875,15 @@ tone_alarm_main(int argc, char *argv[]) if (*argv1 == 'M') { ret = play_string(argv1, false); } - } - else { + + } else { /* It might be a tune name */ for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) if (!strcmp(g_dev->name(tune), argv1)) { ret = play_tune(tune); return ret; } + PX4_WARN("unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); ret = 1; } diff --git a/src/platforms/posix/include/board_config.h b/src/platforms/posix/include/board_config.h index a03b8c3e20b2..6413bb780ba0 100644 --- a/src/platforms/posix/include/board_config.h +++ b/src/platforms/posix/include/board_config.h @@ -9,3 +9,5 @@ #define PX4_I2C_BUS_LED 3 #define PX4_I2C_OBDEV_LED 0x55 + +#define STM32_SYSMEM_UID "SIMULATIONID" diff --git a/src/platforms/posix/include/crc32.h b/src/platforms/posix/include/crc32.h index bf828e3e0e9f..34e080b1c2e3 100644 --- a/src/platforms/posix/include/crc32.h +++ b/src/platforms/posix/include/crc32.h @@ -63,7 +63,7 @@ extern "C" { ****************************************************************************/ EXTERN uint32_t crc32part(const uint8_t *src, size_t len, - uint32_t crc32val); + uint32_t crc32val); /**************************************************************************** * Name: crc32 diff --git a/src/platforms/posix/include/hrt_work.h b/src/platforms/posix/include/hrt_work.h new file mode 100644 index 000000000000..47cdf5872ad2 --- /dev/null +++ b/src/platforms/posix/include/hrt_work.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ + +#include +#include +#include +#include + +#pragma once + +__BEGIN_DECLS + +extern px4_sem_t _hrt_work_lock; +extern struct wqueue_s g_hrt_work; + +void hrt_work_queue_init(void); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); +void hrt_work_cancel(struct work_s *work); + +static inline void hrt_work_lock(void); +static inline void hrt_work_lock() +{ + px4_sem_wait(&_hrt_work_lock); +} + +static inline void hrt_work_unlock(void); +static inline void hrt_work_unlock() +{ + px4_sem_post(&_hrt_work_lock); +} + +__END_DECLS + diff --git a/src/platforms/posix/include/px4_platform_types.h b/src/platforms/posix/include/px4_platform_types.h new file mode 100644 index 000000000000..abaf177444a1 --- /dev/null +++ b/src/platforms/posix/include/px4_platform_types.h @@ -0,0 +1,3 @@ +#ifdef __PX4_QURT +#include +#endif diff --git a/src/platforms/posix/include/queue.h b/src/platforms/posix/include/queue.h index 4d95541e0203..30dc264f3f37 100644 --- a/src/platforms/posix/include/queue.h +++ b/src/platforms/posix/include/queue.h @@ -69,30 +69,26 @@ * Global Type Declarations ************************************************************************/ -struct sq_entry_s -{ - FAR struct sq_entry_s *flink; +struct sq_entry_s { + FAR struct sq_entry_s *flink; }; typedef struct sq_entry_s sq_entry_t; -struct dq_entry_s -{ - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; }; typedef struct dq_entry_s dq_entry_t; -struct sq_queue_s -{ - FAR sq_entry_t *head; - FAR sq_entry_t *tail; +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; }; typedef struct sq_queue_s sq_queue_t; -struct dq_queue_s -{ - FAR dq_entry_t *head; - FAR dq_entry_t *tail; +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; }; typedef struct dq_queue_s dq_queue_t; @@ -112,11 +108,11 @@ EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); + sq_queue_t *queue); EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index f756749d52b0..573483946096 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -32,9 +32,10 @@ ****************************************************************************/ /** * @file main.cpp - * Basic shell to execute builtin "apps" + * Basic shell to execute builtin "apps" * * @author Mark Charlebois + * @auther Roman Bapst */ #include @@ -42,8 +43,11 @@ #include #include #include +#include +#include -namespace px4 { +namespace px4 +{ void init_once(void); } @@ -54,58 +58,251 @@ typedef int (*px4_main_t)(int argc, char *argv[]); #include "apps.h" #include "px4_middleware.h" -static void run_cmd(const vector &appargs) { +#define CMD_BUFF_SIZE 100 + +static bool _ExitFlag = false; +extern "C" { + void _SigIntHandler(int sig_num); + void _SigIntHandler(int sig_num) + { + cout.flush(); + cout << endl << "Exiting.." << endl; + cout.flush(); + _exit(0); + } + void _SigFpeHandler(int sig_num); + void _SigFpeHandler(int sig_num) + { + cout.flush(); + cout << endl << "floating point exception" << endl; + cout.flush(); + } +} + +static void print_prompt() +{ + cout.flush(); + cout << "pxh> "; + cout.flush(); +} + +static void run_cmd(const vector &appargs, bool exit_on_fail) +{ // command is appargs[0] string command = appargs[0]; - cout << "----------------------------------\n"; + if (apps.find(command) != apps.end()) { - const char *arg[appargs.size()+2]; + const char *arg[appargs.size() + 2]; unsigned int i = 0; + while (i < appargs.size() && appargs[i] != "") { arg[i] = (char *)appargs[i].c_str(); ++i; } + arg[i] = (char *)0; - cout << "Running: " << command << "\n"; - apps[command](i,(char **)arg); - } - else - { - cout << "Invalid command: " << command << endl; + int retval = apps[command](i, (char **)arg); + + if (exit_on_fail && retval) { + exit(retval); + } + + usleep(65000); + + } else if (command.compare("help") == 0) { list_builtins(); + + } else if (command.length() == 0) { + // Do nothing + + } else { + cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; + } + + print_prompt(); } -static void process_line(string &line) +static void usage() { - vector appargs(5); - stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4]; - run_cmd(appargs); + cout << "./mainapp [-d] [startup_config] -h" << std::endl; + cout << " -d - Optional flag to run the app in daemon mode and does not take listen for user input." << + std::endl; + cout << " This is needed if mainapp is intended to be run as a upstart job on linux" << std::endl; + cout << " - config file for starting/stopping px4 modules" << std::endl; + cout << " -h - help/usage information" << std::endl; +} + +static void process_line(string &line, bool exit_on_fail) +{ + if (line.length() == 0) { + printf("\n"); + } + + vector appargs(8); + + stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> + appargs[7]; + run_cmd(appargs, exit_on_fail); } int main(int argc, char **argv) { - px4::init_once(); + bool daemon_mode = false; + signal(SIGINT, _SigIntHandler); + signal(SIGFPE, _SigFpeHandler); - px4::init(argc, argv, "mainapp"); + int index = 1; + bool error_detected = false; + char *commands_file = nullptr; - // Execute a command list of provided - if (argc == 2) { - ifstream infile(argv[1]); + while (index < argc) { + if (argv[index][0] == '-') { + // the arg starts with - + if (strcmp(argv[index], "-d") == 0) { + daemon_mode = true; - for (string line; getline(infile, line, '\n'); ) { - process_line(line); + } else if (strcmp(argv[index], "-h") == 0) { + usage(); + return 0; + + } else { + PX4_WARN("Unknown/unhandled parameter: %s", argv[index]); + return 1; + } + + } else { + // this is an argument that does not have '-' prefix; treat it like a file name + ifstream infile(argv[index]); + + if (infile.good()) { + infile.close(); + commands_file = argv[index]; + + } else { + PX4_WARN("Error opening file: %s", argv[index]); + error_detected = true; + break; + } } + + ++index; } - string mystr; - - while(1) { - cout << "Enter a command and its args:" << endl; - getline (cin,mystr); - process_line(mystr); - mystr = ""; + if (!error_detected) { + px4::init_once(); + + px4::init(argc, argv, "mainapp"); + + //if commandfile is present, process the commands from the file + if (commands_file != nullptr) { + ifstream infile(commands_file); + + if (infile.is_open()) { + for (string line; getline(infile, line, '\n');) { + process_line(line, false); + } + + } else { + PX4_WARN("Error opening file: %s", commands_file); + } + } + + if (!daemon_mode) { + string mystr = ""; + string string_buffer[CMD_BUFF_SIZE]; + int buf_ptr_write = 0; + int buf_ptr_read = 0; + + print_prompt(); + + // change input mode so that we can manage shell + struct termios term; + tcgetattr(0, &term); + term.c_lflag &= ~ICANON; + term.c_lflag &= ~ECHO; + tcsetattr(0, TCSANOW, &term); + setbuf(stdin, NULL); + + while (!_ExitFlag) { + + char c = getchar(); + + switch (c) { + case 127: // backslash + if (mystr.length() > 0) { + mystr.pop_back(); + printf("%c[2K", 27); // clear line + cout << (char)13; + print_prompt(); + cout << mystr; + } + + break; + + case'\n': // user hit enter + if (buf_ptr_write == CMD_BUFF_SIZE) { + buf_ptr_write = 0; + } + + string_buffer[buf_ptr_write] = mystr; + buf_ptr_write++; + process_line(mystr, !daemon_mode); + mystr = ""; + buf_ptr_read = buf_ptr_write; + break; + + case '\033': { // arrow keys + c = getchar(); // skip first one, does not have the info + c = getchar(); + + if (c == 'A') { + buf_ptr_read--; + + } else if (c == 'B') { + if (buf_ptr_read < buf_ptr_write) { + buf_ptr_read++; + } + + } else { + // TODO: Support editing current line + } + + if (buf_ptr_read < 0) { + buf_ptr_read = 0; + } + + string saved_cmd = string_buffer[buf_ptr_read]; + printf("%c[2K", 27); + cout << (char)13; + mystr = saved_cmd; + print_prompt(); + cout << mystr; + break; + } + + default: // any other input + cout << c; + mystr += c; + break; + } + } + + } else { + while (!_ExitFlag) { + usleep(100000); + } + } + + if (px4_task_is_running("muorb")) { + // sending muorb stop is needed if it is running to exit cleanly + vector muorb_stop_cmd = { "muorb", "stop" }; + run_cmd(muorb_stop_cmd, !daemon_mode); + } + + vector shutdown_cmd = { "shutdown" }; + run_cmd(shutdown_cmd, true); } } diff --git a/src/platforms/posix/px4_layer/CMakeLists.txt b/src/platforms/posix/px4_layer/CMakeLists.txt new file mode 100644 index 000000000000..402e66e097e3 --- /dev/null +++ b/src/platforms/posix/px4_layer/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__px4_layer + COMPILE_FLAGS + -Os + SRCS + px4_posix_impl.cpp + px4_posix_tasks.cpp + px4_sem.cpp + lib_crc32.c + drv_hrt.c + px4_log.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index f5c351b187df..e4839f0ac771 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -37,12 +37,16 @@ * High-resolution timer with callouts and timekeeping. */ +#include #include #include #include #include #include -#include +#define __STDC_FORMAT_MACROS +#include +#include +#include "hrt_work.h" static struct sq_queue_s callout_queue; @@ -54,28 +58,57 @@ __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; static void hrt_call_reschedule(void); -// Intervals in ms +// Intervals in usec #define HRT_INTERVAL_MIN 50 -#define HRT_INTERVAL_MAX 50000 +#define HRT_INTERVAL_MAX 50000000 -static sem_t _hrt_lock; +static px4_sem_t _hrt_lock; static struct work_s _hrt_work; +static hrt_abstime px4_timestart = 0; static void hrt_call_invoke(void); +__EXPORT hrt_abstime hrt_reset(void); + static void hrt_lock(void) { - //printf("hrt_lock\n"); - sem_wait(&_hrt_lock); + px4_sem_wait(&_hrt_lock); } static void hrt_unlock(void) { - //printf("hrt_unlock\n"); - sem_post(&_hrt_lock); + px4_sem_post(&_hrt_lock); +} + +#if defined(__APPLE__) && defined(__MACH__) +#include +#include +#define CLOCK_REALTIME 0 + +int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) +{ + struct timeval now; + int rv = gettimeofday(&now, NULL); + + if (rv) { + return rv; + } + + tp->tv_sec = now.tv_sec; + tp->tv_nsec = now.tv_usec * 1000; + + return 0; } +int px4_clock_settime(clockid_t clk_id, struct timespec *tp) +{ + /* do nothing right now */ + return 0; +} + +#endif + /* * Get absolute time. */ @@ -83,8 +116,19 @@ hrt_abstime hrt_absolute_time(void) { struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - return ts_to_abstime(&ts); + if (!px4_timestart) { + px4_clock_gettime(CLOCK_MONOTONIC, &ts); + px4_timestart = ts_to_abstime(&ts); + } + + px4_clock_gettime(CLOCK_MONOTONIC, &ts); + return ts_to_abstime(&ts) - px4_timestart; +} + +__EXPORT hrt_abstime hrt_reset(void) +{ + px4_timestart = 0; + return hrt_absolute_time(); } /* @@ -178,9 +222,14 @@ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) */ void hrt_init(void) { - //printf("hrt_init\n"); sq_init(&callout_queue); - sem_init(&_hrt_lock, 0, 1); + + int sem_ret = px4_sem_init(&_hrt_lock, 0, 1); + + if (sem_ret) { + PX4_ERR("SEM INIT FAIL: %s", strerror(errno)); + } + memset(&_hrt_work, 0, sizeof(_hrt_work)); } @@ -189,12 +238,12 @@ hrt_call_enter(struct hrt_call *entry) { struct hrt_call *call, *next; - //printf("hrt_call_enter\n"); + //PX4_INFO("hrt_call_enter"); call = (struct hrt_call *)sq_peek(&callout_queue); if ((call == NULL) || (entry->deadline < call->deadline)) { sq_addfirst(&entry->link, &callout_queue); - //lldbg("call enter at head, reschedule\n"); + //if (call != NULL) PX4_INFO("call enter at head, reschedule (%lu %lu)", entry->deadline, call->deadline); /* we changed the next deadline, reschedule the timer event */ hrt_call_reschedule(); @@ -210,7 +259,7 @@ hrt_call_enter(struct hrt_call *entry) } while ((call = next) != NULL); } - //lldbg("scheduled\n"); + //PX4_INFO("scheduled"); } /** @@ -218,11 +267,11 @@ hrt_call_enter(struct hrt_call *entry) * * This routine simulates a timer interrupt handler */ -static void +static void hrt_tim_isr(void *p) { - //printf("hrt_tim_isr\n"); + //PX4_INFO("hrt_tim_isr"); /* run any callouts that have met their deadline */ hrt_call_invoke(); @@ -243,12 +292,12 @@ static void hrt_call_reschedule() { hrt_abstime now = hrt_absolute_time(); + hrt_abstime delay = HRT_INTERVAL_MAX; struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); hrt_abstime deadline = now + HRT_INTERVAL_MAX; - uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); - //printf("hrt_call_reschedule\n"); - + //PX4_INFO("hrt_call_reschedule"); + /* * Determine what the next deadline will be. * @@ -266,26 +315,30 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); + delay = HRT_INTERVAL_MIN; } else if (next->deadline < deadline) { //lldbg("due soon\n"); - ticks = USEC2TICK((next->deadline - now)*1000); + delay = next->deadline - now; } } - // There is no timer ISR, so simulate one by putting an event on the + // There is no timer ISR, so simulate one by putting an event on the // high priority work queue - //printf("ticks = %u\n", ticks); - work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); + + // Remove the existing expiry and update with the new expiry + hrt_work_cancel(&_hrt_work); + + hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, NULL, delay); } static void hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) { - //printf("hrt_call_internal\n"); + PX4_DEBUG("hrt_call_internal deadline=%lu interval = %lu", deadline, interval); hrt_lock(); - //printf("hrt_call_internal after lock\n"); + + //PX4_INFO("hrt_call_internal after lock"); /* if the entry is currently queued, remove it */ /* note that we are using a potentially uninitialised entry->link here, but it is safe as sq_rem() doesn't @@ -294,9 +347,19 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte queue for the uninitialised entry->link but we don't do anything actually unsafe. */ - if (entry->deadline != 0) + if (entry->deadline != 0) { sq_rem(&entry->link, &callout_queue); + } + +#if 1 + // Use this to debug busy CPU that keeps rescheduling with 0 period time + /*if (interval < HRT_INTERVAL_MIN) {*/ + /*PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);*/ + /*PX4_BACKTRACE();*/ + /*}*/ + +#endif entry->deadline = deadline; entry->period = interval; entry->callout = callout; @@ -359,20 +422,23 @@ hrt_call_invoke(void) hrt_abstime deadline; hrt_lock(); + while (true) { /* get the current time */ hrt_abstime now = hrt_absolute_time(); call = (struct hrt_call *)sq_peek(&callout_queue); - if (call == NULL) + if (call == NULL) { break; + } - if (call->deadline > now) + if (call->deadline > now) { break; + } sq_rem(&call->link, &callout_queue); - //lldbg("call pop\n"); + //PX4_INFO("call pop"); /* save the intended deadline for periodic calls */ deadline = call->deadline; @@ -385,7 +451,7 @@ hrt_call_invoke(void) // Unlock so we don't deadlock in callback hrt_unlock(); - //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); + //PX4_INFO("call %p: %p(%p)", call, call->callout, call->arg); call->callout(call->arg); hrt_lock(); @@ -398,11 +464,13 @@ hrt_call_invoke(void) // using hrt_call_delay() if (call->deadline <= now) { call->deadline = deadline + call->period; + //PX4_INFO("call deadline set to %lu now=%lu", call->deadline, now); } hrt_call_enter(call); } } + hrt_unlock(); } diff --git a/src/platforms/posix/px4_layer/lib_crc32.c b/src/platforms/posix/px4_layer/lib_crc32.c index 4ba6fbf6df79..c14ebfceeba2 100644 --- a/src/platforms/posix/px4_layer/lib_crc32.c +++ b/src/platforms/posix/px4_layer/lib_crc32.c @@ -48,46 +48,45 @@ #include // Needed for Linux -#define FAR +#define FAR /************************************************************************************************ * Private Data ************************************************************************************************/ - -static const uint32_t crc32_tab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d + +static const uint32_t crc32_tab[] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d }; /************************************************************************************************ @@ -103,13 +102,13 @@ static const uint32_t crc32_tab[] = uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) { - size_t i; + size_t i; + + for (i = 0; i < len; i++) { + crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); + } - for (i = 0; i < len; i++) - { - crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); - } - return crc32val; + return crc32val; } /************************************************************************************************ @@ -122,5 +121,5 @@ uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) uint32_t crc32(FAR const uint8_t *src, size_t len) { - return crc32part(src, len, 0); + return crc32part(src, len, 0); } diff --git a/src/platforms/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk index 2df90461da82..846ce9469cea 100644 --- a/src/platforms/posix/px4_layer/module.mk +++ b/src/platforms/posix/px4_layer/module.mk @@ -32,23 +32,14 @@ ############################################################################ # -# NuttX / uORB adapter library +# POSIX platform dependent files # SRCS = \ px4_posix_impl.cpp \ px4_posix_tasks.cpp \ - work_thread.c \ - work_queue.c \ - work_cancel.c \ lib_crc32.c \ drv_hrt.c \ - queue.c \ - dq_addlast.c \ - dq_remfirst.c \ - sq_addlast.c \ - sq_remfirst.c \ - sq_addafter.c \ - dq_rem.c + px4_log.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c new file mode 100644 index 000000000000..074ae8b466de --- /dev/null +++ b/src/platforms/posix/px4_layer/px4_log.c @@ -0,0 +1,30 @@ +#include +#include +#ifdef __PX4_POSIX +#include +#endif + +__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; + +__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" }; + +void px4_backtrace() +{ +#ifdef __PX4_POSIX + void *buffer[10]; + char **callstack; + int bt_size; + int idx; + + bt_size = backtrace(buffer, 10); + callstack = backtrace_symbols(buffer, bt_size); + + PX4_INFO("Backtrace: %d", bt_size); + + for (idx = 0; idx < bt_size; idx++) { + PX4_INFO("%s", callstack[idx]); + } + + free(callstack); +#endif +} diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 3f1916a51ad2..19747556a9a3 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -46,13 +46,16 @@ #include #include #include "systemlib/param/param.h" +#include "hrt_work.h" +#include +#include "px4_time.h" + +extern pthread_t _shell_task_id; __BEGIN_DECLS long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); -extern void hrt_init(void); - __END_DECLS namespace px4 @@ -62,13 +65,32 @@ void init_once(void); void init_once(void) { + _shell_task_id = pthread_self(); + printf("[init] shell id: %lu\n", (unsigned long)_shell_task_id); work_queues_init(); + hrt_work_queue_init(); hrt_init(); } void init(int argc, char *argv[], const char *app_name) { - printf("App name: %s\n", app_name); + printf("[init] task name: %s\n", app_name); + printf("\n"); + printf("______ __ __ ___ \n"); + printf("| ___ \\ \\ \\ / / / |\n"); + printf("| |_/ / \\ V / / /| |\n"); + printf("| __/ / \\ / /_| |\n"); + printf("| | / /^\\ \\ \\___ |\n"); + printf("\\_| \\/ \\/ |_/\n"); + printf("\n"); + printf("Ready to fly.\n"); + printf("\n"); + printf("\n"); +} + +uint64_t get_time_micros() +{ + return hrt_absolute_time(); } } diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 5a36313aedeb..41cade1d5da4 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -54,12 +54,16 @@ #include #include +#include #define MAX_CMD_LEN 100 -#define PX4_MAX_TASKS 100 -struct task_entry -{ +#define PX4_MAX_TASKS 50 +#define SHELL_TASK_ID (PX4_MAX_TASKS+1) + +pthread_t _shell_task_id = 0; + +struct task_entry { pthread_t pid; std::string name; bool isused; @@ -68,35 +72,34 @@ struct task_entry static task_entry taskmap[PX4_MAX_TASKS]; -typedef struct -{ +typedef struct { px4_main_t entry; int argc; char *argv[]; - // strings are allocated after the + // strings are allocated after the } pthdata_t; -static void *entry_adapter ( void *ptr ) +static void *entry_adapter(void *ptr) { - pthdata_t *data; - data = (pthdata_t *) ptr; - + pthdata_t *data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); free(ptr); PX4_DEBUG("Before px4_task_exit"); - px4_task_exit(0); + px4_task_exit(0); PX4_DEBUG("After px4_task_exit"); return NULL; -} +} void px4_systemreset(bool to_bootloader) { PX4_WARN("Called px4_system_reset"); + exit(0); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, + char *const argv[]) { int rv; int argc = 0; @@ -104,50 +107,62 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p = (char *)argv; + char *p = (char *)argv; - pthread_t task; + pthread_t task; pthread_attr_t attr; struct sched_param param; // Calculate argc while (p != (char *)0) { p = argv[argc]; - if (p == (char *)0) + + if (p == (char *)0) { break; + } + ++argc; - len += strlen(p)+1; + len += strlen(p) + 1; } - structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); + + structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); pthdata_t *taskdata; - + // not safe to pass stack data to the thread creation - taskdata = (pthdata_t *)malloc(structsize+len); - offset = ((unsigned long)taskdata)+structsize; + taskdata = (pthdata_t *)malloc(structsize + len); + offset = ((unsigned long)taskdata) + structsize; - taskdata->entry = entry; + taskdata->entry = entry; taskdata->argc = argc; - for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); - offset+=strlen(argv[i])+1; + offset += strlen(argv[i]) + 1; } + // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; + PX4_DEBUG("starting task %s", name); + rv = pthread_attr_init(&attr); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); return (rv < 0) ? rv : -rv; } + rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); return (rv < 0) ? rv : -rv; } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); return (rv < 0) ? rv : -rv; @@ -156,28 +171,31 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int param.sched_priority = priority; rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); return (rv < 0) ? rv : -rv; } - rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); + rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); + if (rv != 0) { if (rv == EPERM) { //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); - rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); + rv = pthread_create(&task, NULL, &entry_adapter, (void *) taskdata); + if (rv != 0) { PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); return (rv < 0) ? rv : -rv; } - } - else { + + } else { return (rv < 0) ? rv : -rv; } } - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { return -ENOSPC; } - return i; + + return i; } int px4_task_delete(px4_task_t id) { int rv = 0; pthread_t pid; - PX4_WARN("Called px4_task_delete"); + PX4_DEBUG("Called px4_task_delete"); - if (id < PX4_MAX_TASKS && taskmap[id].isused) + if (id < PX4_MAX_TASKS && taskmap[id].isused) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { + if (pthread_self() == pid) { taskmap[id].isused = false; pthread_exit(0); + } else { rv = pthread_cancel(pid); } @@ -217,20 +240,21 @@ int px4_task_delete(px4_task_t id) void px4_task_exit(int ret) { - int i; + int i; pthread_t pid = pthread_self(); // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { PX4_ERR("px4_task_exit: self task not found!"); - } - else { + + } else { PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); } @@ -243,10 +267,12 @@ int px4_task_kill(px4_task_t id, int sig) pthread_t pid; PX4_DEBUG("Called px4_task_kill %d", sig); - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) + if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel rv = pthread_kill(pid, sig); @@ -260,29 +286,50 @@ void px4_show_tasks() int count = 0; PX4_INFO("Active Tasks:"); - for (idx=0; idx < PX4_MAX_TASKS; idx++) - { + + for (idx = 0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - PX4_INFO(" %-10s %lu", taskmap[idx].name.c_str(), taskmap[idx].pid); + PX4_INFO(" %-10s %lu", taskmap[idx].name.c_str(), (unsigned long)taskmap[idx].pid); count++; } } - if (count == 0) + + if (count == 0) { PX4_INFO(" No running tasks"); + } } +bool px4_task_is_running(const char *taskname) +{ + int idx; + + for (idx = 0; idx < PX4_MAX_TASKS; idx++) { + if (taskmap[idx].isused && (strcmp(taskmap[idx].name.c_str(), taskname) == 0)) { + return true; + } + } + + return false; +} __BEGIN_DECLS + +unsigned long px4_getpid() +{ + return (unsigned long)pthread_self(); +} + +const char *getprogname(); const char *getprogname() { - pthread_t pid = pthread_self(); - for (int i=0; i +#include +#include +#include +#include +#include + +#ifdef __PX4_DARWIN + +#include + +#include + +int px4_sem_init(px4_sem_t *s, int pshared, unsigned value) +{ + // We do not used the process shared arg + (void)pshared; + s->value = value; + pthread_cond_init(&(s->wait), NULL); + pthread_mutex_init(&(s->lock), NULL); + + return 0; +} + +int px4_sem_wait(px4_sem_t *s) +{ + pthread_mutex_lock(&(s->lock)); + s->value--; + + if (s->value < 0) { + pthread_cond_wait(&(s->wait), &(s->lock)); + } + + pthread_mutex_unlock(&(s->lock)); + + return 0; +} + +int px4_sem_post(px4_sem_t *s) +{ + pthread_mutex_lock(&(s->lock)); + s->value++; + + if (s->value <= 0) { + pthread_cond_signal(&(s->wait)); + } + + pthread_mutex_unlock(&(s->lock)); + + return 0; +} + +int px4_sem_getvalue(px4_sem_t *s, int *sval) +{ + pthread_mutex_lock(&(s->lock)); + *sval = s->value; + pthread_mutex_unlock(&(s->lock)); + + return 0; +} + +int px4_sem_destroy(px4_sem_t *s) +{ + pthread_mutex_lock(&(s->lock)); + pthread_cond_destroy(&(s->wait)); + pthread_mutex_unlock(&(s->lock)); + pthread_mutex_destroy(&(s->lock)); + + return 0; +} + +#endif diff --git a/src/platforms/posix/tests/hello/CMakeLists.txt b/src/platforms/posix/tests/hello/CMakeLists.txt new file mode 100644 index 000000000000..f19e4eb8728b --- /dev/null +++ b/src/platforms/posix/tests/hello/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__tests__hello + MAIN hello + SRCS + hello_main.cpp + hello_start_posix.cpp + hello_example.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/tests/hello/hello_example.cpp b/src/platforms/posix/tests/hello/hello_example.cpp index a30aeb57bc53..5d3cb76852fc 100644 --- a/src/platforms/posix/tests/hello/hello_example.cpp +++ b/src/platforms/posix/tests/hello/hello_example.cpp @@ -49,8 +49,9 @@ int HelloExample::main() { appState.setRunning(true); - int i=0; - while (!appState.exitRequested() && i<5) { + int i = 0; + + while (!appState.exitRequested() && i < 5) { sleep(2); printf(" Doing work...\n"); diff --git a/src/platforms/posix/tests/hello/hello_example.h b/src/platforms/posix/tests/hello/hello_example.h index a4ae51705637..bf589996d1a3 100644 --- a/src/platforms/posix/tests/hello/hello_example.h +++ b/src/platforms/posix/tests/hello/hello_example.h @@ -41,7 +41,8 @@ #include -class HelloExample { +class HelloExample +{ public: HelloExample() {}; diff --git a/src/platforms/posix/tests/hello/hello_start_posix.cpp b/src/platforms/posix/tests/hello/hello_start_posix.cpp index 8dde729a6e82..583baaf56ae4 100644 --- a/src/platforms/posix/tests/hello/hello_start_posix.cpp +++ b/src/platforms/posix/tests/hello/hello_start_posix.cpp @@ -55,7 +55,7 @@ static int daemon_task; /* Handle of deamon task / thread */ extern "C" __EXPORT int hello_main(int argc, char *argv[]); int hello_main(int argc, char *argv[]) { - + if (argc < 2) { PX4_WARN("usage: hello {start|stop|status}\n"); return 1; @@ -70,11 +70,11 @@ int hello_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("hello", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/hrt_test/CMakeLists.txt b/src/platforms/posix/tests/hrt_test/CMakeLists.txt new file mode 100644 index 000000000000..3a67f630ea42 --- /dev/null +++ b/src/platforms/posix/tests/hrt_test/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__tests__hrt_test + MAIN hrttest + SRCS + hrt_test_main.cpp + hrt_test_start_posix.cpp + hrt_test.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp index 8dd1f4bde3ff..528980cea8e1 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test.cpp @@ -54,6 +54,7 @@ static void timer_expired(void *arg) { static int i = 0; PX4_INFO("Test\n"); + if (i < 5) { i++; hrt_call_after(&t1, update_interval, timer_expired, (void *)0); @@ -79,7 +80,7 @@ int HRTTest::main() memset(&t1, 0, sizeof(t1)); PX4_INFO("HRT_CALL %d\n", hrt_called(&t1)); - + hrt_call_after(&t1, update_interval, timer_expired, (void *)0); sleep(2); PX4_INFO("HRT_CALL - %d\n", hrt_called(&t1)); diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.h b/src/platforms/posix/tests/hrt_test/hrt_test.h index c4c97be6d205..ac4450e919fd 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.h +++ b/src/platforms/posix/tests/hrt_test/hrt_test.h @@ -41,7 +41,8 @@ #include -class HRTTest { +class HRTTest +{ public: HRTTest() {}; diff --git a/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp index 2544804496ed..ed718b2cc4d3 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp @@ -63,11 +63,11 @@ int hrttest_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("hrttest", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/muorb/CMakeLists.txt b/src/platforms/posix/tests/muorb/CMakeLists.txt new file mode 100644 index 000000000000..967a598cb46f --- /dev/null +++ b/src/platforms/posix/tests/muorb/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__tests__muorb + MAIN muorb_test + SRCS + muorb_test_main.cpp + muorb_test_start_posix.cpp + muorb_test_example.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/tests/muorb/module.mk b/src/platforms/posix/tests/muorb/module.mk new file mode 100644 index 000000000000..880fe820078d --- /dev/null +++ b/src/platforms/posix/tests/muorb/module.mk @@ -0,0 +1,47 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = muorb_test + +INCLUDE_DIRS += \ + $(EXT_MUORB_LIB_ROOT)/krait/include \ + $(PX4_BASE)src/modules/muorb/krait + +SRCS = muorb_test_main.cpp \ + muorb_test_start_posix.cpp \ + muorb_test_example.cpp + diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.cpp b/src/platforms/posix/tests/muorb/muorb_test_example.cpp new file mode 100644 index 000000000000..4547c6db5701 --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_example.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 hello_example.cpp + * Example for Linux + * + * @author Mark Charlebois + */ + +#include "muorb_test_example.h" +#include +#include +#include +#include + +px4::AppState MuorbTestExample::appState; + +int MuorbTestExample::main() +{ + int rc; + appState.setRunning(true); + rc = PingPongTest(); + appState.setRunning(false); + return rc; +} + +int MuorbTestExample::DefaultTest() +{ + int i = 0; + orb_advert_t pub_id = orb_advertise(ORB_ID(esc_status), & m_esc_status); + + if (pub_id == 0) { + PX4_ERR("error publishing esc_status"); + return -1; + } + + orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc); + + if (pub_id_vc == 0) { + PX4_ERR("error publishing vehicle_command"); + return -1; + } + + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + return -1; + } + + int sub_vc = orb_subscribe(ORB_ID(vehicle_command)); + + if (sub_vc == PX4_ERROR) { + PX4_ERR("Error subscribing to vehicle_command topic"); + return -1; + } + + while (!appState.exitRequested() && i < 100) { + + PX4_DEBUG("[%d] Doing work...", i); + + if (orb_publish(ORB_ID(esc_status), pub_id, &m_esc_status) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the esc status message for iter", i); + break; + } + + bool updated = false; + + if (orb_check(sub_vc, &updated) == 0) { + if (updated) { + PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i); + + if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) { + PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i); + break; + } + + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; + } + + } else { + PX4_DEBUG("[%d] VC topic is not updated", i); + } + + } else { + PX4_ERR("[%d]Error checking the updated status for vechile command... ", i); + break; + } + + ++i; + } + + return 0; +} + +int MuorbTestExample::PingPongTest() +{ + int i = 0; + orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc); + + if (pub_id_vc == 0) { + PX4_ERR("error publishing vehicle_command"); + return -1; + } + + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + return -1; + } + + int sub_esc_status = orb_subscribe(ORB_ID(esc_status)); + + if (sub_esc_status == PX4_ERROR) { + PX4_ERR("Error subscribing to esc_status topic"); + return -1; + } + + while (!appState.exitRequested()) { + + PX4_INFO("[%d] Doing work...", i); + bool updated = false; + + if (orb_check(sub_esc_status, &updated) == 0) { + if (updated) { + PX4_INFO("[%d]ESC status is updated... reading new value", i); + + if (orb_copy(ORB_ID(esc_status), sub_esc_status, &m_esc_status) != 0) { + PX4_ERR("[%d]Error calling orb copy for esc status... ", i); + break; + } + + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; + } + + } else { + PX4_INFO("[%d] esc status topic is not updated", i); + } + + } else { + PX4_ERR("[%d]Error checking the updated status for esc status... ", i); + break; + } + + // sleep for 1 sec. + usleep(1000000); + + ++i; + } + + return 0; +} diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.h b/src/platforms/posix/tests/muorb/muorb_test_example.h new file mode 100644 index 000000000000..4f4dcf02d6f1 --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_example.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 hello_example.h + * Example app for Linux + * + * @author Mark Charlebois + */ +#pragma once + +#include +#include "uORB/topics/esc_status.h" +#include "uORB/topics/vehicle_command.h" + +class MuorbTestExample +{ +public: + MuorbTestExample() {}; + + ~MuorbTestExample() {}; + + int main(); + + static px4::AppState appState; /* track requests to terminate app */ +private: + int DefaultTest(); + int PingPongTest(); + struct esc_status_s m_esc_status; + struct vehicle_command_s m_vc; + +}; diff --git a/src/platforms/posix/tests/muorb/muorb_test_main.cpp b/src/platforms/posix/tests/muorb/muorb_test_main.cpp new file mode 100644 index 000000000000..d9e9d1c6a9ff --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_main.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 hello_main.cpp + * Example for Linux + * + * @author Mark Charlebois + */ +#include +#include +#include +#include "muorb_test_example.h" +#include +#include "uORB/uORBManager.hpp" +#include "uORBKraitFastRpcChannel.hpp" + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "muorb_test"); + + PX4_DEBUG("muorb_test"); + + MuorbTestExample hello; + hello.main(); + + PX4_DEBUG("goodbye"); + return 0; +} diff --git a/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp b/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp new file mode 100644 index 000000000000..20227ba54ede --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 hello_start_linux.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "muorb_test_example.h" +#include +#include +#include +#include +#include +#include + +static int daemon_task; /* Handle of deamon task / thread */ + +//using namespace px4; + +extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]); + +static void usage() +{ + PX4_DEBUG("usage: muorb_test {start|stop|status}"); +} +int muorb_test_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("already running"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("muorb_test", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char *const *)argv); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + MuorbTestExample::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("is running"); + + } else { + PX4_DEBUG("not started"); + } + + return 0; + } + + usage(); + return 1; +} diff --git a/src/platforms/posix/tests/vcdev_test/CMakeLists.txt b/src/platforms/posix/tests/vcdev_test/CMakeLists.txt new file mode 100644 index 000000000000..b7ff2cc9e219 --- /dev/null +++ b/src/platforms/posix/tests/vcdev_test/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__tests__vcdev_test + MAIN vcdevtest + SRCS + vcdevtest_main.cpp + vcdevtest_start_posix.cpp + vcdevtest_example.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index dc3111a40a84..1276b740a24e 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -51,55 +51,141 @@ px4::AppState VCDevExample::appState; using namespace device; -#define TESTDEV "/dev/vdevex" +#define TESTDEV "/dev/vdevtest" + +static bool g_exit = false; static int writer_main(int argc, char *argv[]) { - char buf[1] = { '1' }; + char buf[1]; + + int fd = px4_open(TESTDEV, PX4_F_WRONLY); - int fd = px4_open(TESTDEV, PX4_F_RDONLY); if (fd < 0) { - PX4_INFO("--- Open failed %d %d", fd, px4_errno); + PX4_INFO("Writer: Open failed %d %d", fd, px4_errno); return -px4_errno; } int ret; - int i=0; - while (i<3) { - // Wait for 3 seconds - PX4_INFO("--- Sleeping for 4 sec\n"); - ret = sleep(4); + int i = 0; + + while (!g_exit) { + // Wait for 2 seconds + PX4_INFO("Writer: Sleeping for 2 sec"); + ret = sleep(2); + if (ret < 0) { - PX4_INFO("--- sleep failed %d %d\n", ret, errno); + PX4_INFO("Writer: sleep failed %d %d", ret, errno); return ret; } - PX4_INFO("--- writing to fd\n"); + buf[0] = 'A' + (char)(i % 26); + PX4_INFO("Writer: writing char '%c'", buf[0]); ret = px4_write(fd, buf, 1); - ++i; + ++i; } + px4_close(fd); + PX4_INFO("Writer: stopped"); return ret; } -class VCDevNode : public VDev { +class PrivData +{ +public: + PrivData() : _read_offset(0) {} + ~PrivData() {} + + size_t _read_offset; +}; + +class VCDevNode : public VDev +{ public: - VCDevNode() : VDev("vcdevtest", TESTDEV) {}; + VCDevNode() : + VDev("vcdevtest", TESTDEV), + _is_open_for_write(false), + _write_offset(0) {}; ~VCDevNode() {} + virtual int open(device::file_t *handlep); + virtual int close(device::file_t *handlep); virtual ssize_t write(device::file_t *handlep, const char *buffer, size_t buflen); + virtual ssize_t read(device::file_t *handlep, char *buffer, size_t buflen); +private: + bool _is_open_for_write; + size_t _write_offset; + char _buf[1000]; }; +int VCDevNode::open(device::file_t *handlep) +{ + // Only allow one writer + if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) { + errno = EBUSY; + return -1; + } + + int ret = VDev::open(handlep); + + if (ret != 0) { + return ret; + } + + handlep->priv = new PrivData; + + if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) { + _is_open_for_write = true; + } + + return 0; +} + +int VCDevNode::close(device::file_t *handlep) +{ + delete(PrivData *)handlep->priv; + handlep->priv = nullptr; + VDev::close(handlep); + + // Enable a new writer of the device is re-opened for write + if ((handlep->flags & PX4_F_WRONLY) && _is_open_for_write) { + _is_open_for_write = false; + } + + return 0; +} + ssize_t VCDevNode::write(device::file_t *handlep, const char *buffer, size_t buflen) { + for (size_t i = 0; i < buflen && _write_offset < 1000; i++) { + _buf[_write_offset] = buffer[i]; + _write_offset++; + } + // ignore what was written, but let pollers know something was written poll_notify(POLLIN); return buflen; } -VCDevExample::~VCDevExample() { +ssize_t VCDevNode::read(device::file_t *handlep, char *buffer, size_t buflen) +{ + PrivData *p = (PrivData *)handlep->priv; + ssize_t chars_read = 0; + PX4_INFO("read %zu write %zu", p->_read_offset, _write_offset); + + for (size_t i = 0; i < buflen && (p->_read_offset < _write_offset); i++) { + buffer[i] = _buf[p->_read_offset]; + p->_read_offset++; + chars_read++; + } + + return chars_read; +} + +VCDevExample::~VCDevExample() +{ if (_node) { delete _node; _node = 0; @@ -109,21 +195,77 @@ VCDevExample::~VCDevExample() { static int test_pub_block(int fd, unsigned long blocked) { int ret = px4_ioctl(fd, DEVIOCSPUBBLOCK, blocked); + if (ret < 0) { PX4_INFO("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } ret = px4_ioctl(fd, DEVIOCGPUBBLOCK, 0); + if (ret < 0) { PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } - PX4_INFO("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); + + PX4_INFO("pub_blocked = %d %s", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); return 0; } +int VCDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after_poll) +{ + int pollret, readret; + int loop_count = 0; + char readbuf[10]; + px4_pollfd_struct_t fds[1]; + + fds[0].fd = fd; + fds[0].events = POLLIN; + fds[0].revents = 0; + + bool mustblock = (timeout < 0); + + // Test indefinte blocking poll + while ((!appState.exitRequested()) && (loop_count < iterations)) { + pollret = px4_poll(fds, 1, timeout); + + if (pollret < 0) { + PX4_ERR("Reader: px4_poll failed %d %d FAIL", pollret, px4_errno); + goto fail; + } + + PX4_INFO("Reader: px4_poll returned %d", pollret); + + if (pollret) { + readret = px4_read(fd, readbuf, 10); + + if (readret != 1) { + if (mustblock) { + PX4_ERR("Reader: read failed %d FAIL", readret); + goto fail; + + } else { + PX4_INFO("Reader: read failed %d FAIL", readret); + } + + } else { + readbuf[readret] = '\0'; + PX4_INFO("Reader: px4_poll returned %d, read '%s' PASS", pollret, readbuf); + } + } + + if (delayms_after_poll) { + usleep(delayms_after_poll * 1000); + } + + loop_count++; + } + + return 0; +fail: + return 1; +} int VCDevExample::main() { appState.setRunning(true); @@ -131,12 +273,12 @@ int VCDevExample::main() _node = new VCDevNode(); if (_node == 0) { - PX4_INFO("Failed to allocate VCDevNode\n"); + PX4_INFO("Failed to allocate VCDevNode"); return -ENOMEM; } if (_node->init() != PX4_OK) { - PX4_INFO("Failed to init VCDevNode\n"); + PX4_INFO("Failed to init VCDevNode"); return 1; } @@ -149,66 +291,69 @@ int VCDevExample::main() void *p = 0; int ret = px4_ioctl(fd, DIOC_GETPRIV, (unsigned long)&p); + if (ret < 0) { PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno); return -px4_errno; } - PX4_INFO("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL"); + + PX4_INFO("priv data = %p %s", p, p == (void *)_node ? "PASS" : "FAIL"); ret = test_pub_block(fd, 1); - if (ret < 0) + + if (ret < 0) { return ret; + } + ret = test_pub_block(fd, 0); - if (ret < 0) + + if (ret < 0) { return ret; + } - int i=0; - px4_pollfd_struct_t fds[1]; - // Start a task that will write something in 3 seconds - (void)px4_task_spawn_cmd("writer", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 6, - 2000, - writer_main, - (char* const*)NULL); - - while (!appState.exitRequested() && i<13) { - PX4_INFO("=====================\n"); - PX4_INFO("==== sleeping 2 sec ====\n"); - sleep(2); - - fds[0].fd = fd; - fds[0].events = POLLIN; - fds[0].revents = 0; - - PX4_INFO("==== Calling Poll\n"); - ret = px4_poll(fds, 1, 1000); - PX4_INFO("==== Done poll\n"); - if (ret < 0) { - PX4_INFO("==== poll failed %d %d\n", ret, px4_errno); - px4_close(fd); - } - else if (i > 0) { - if (ret == 0) { - PX4_INFO("==== Nothing to read - PASS\n"); - } - else { - PX4_INFO("==== poll returned %d\n", ret); - } - } - else if (i == 0) { - if (ret == 1) { - PX4_INFO("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); - } - else { - PX4_INFO("==== %d to read - FAIL\n", ret); - } - - } - ++i; + // Start a task that will write something in 4 seconds + (void)px4_task_spawn_cmd("writer", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 6, + 2000, + writer_main, + (char *const *)NULL); + + ret = 0; + + PX4_INFO("TEST: BLOCKING POLL ---------------"); + + if (do_poll(fd, -1, 3, 0)) { + ret = 1; + goto fail2; } - px4_close(fd); - return 0; + PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); + + if (do_poll(fd, 0, 3, 0)) { + ret = 1; + goto fail2; + } + + PX4_INFO("TEST: 100ms TIMEOUT POLL -----------"); + + if (do_poll(fd, 0, 30, 100)) { + ret = 1; + goto fail2; + } + + PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------"); + + if (do_poll(fd, 1000, 3, 0)) { + ret = 1; + goto fail2; + } + +fail2: + g_exit = true; + px4_close(fd); + PX4_INFO("TEST: waiting for writer to stop"); + sleep(3); + return ret; } diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h index 4898210dffeb..a1cc325f007c 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h @@ -43,7 +43,8 @@ class VCDevNode; -class VCDevExample { +class VCDevExample +{ public: VCDevExample() : _node(0) {}; @@ -54,5 +55,7 @@ class VCDevExample { static px4::AppState appState; /* track requests to terminate app */ private: + int do_poll(int fd, int timeout, int iterations, int delayms_after_poll); + VCDevNode *_node; }; diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp index 5ed9269b2dc9..135ca8f3f713 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp @@ -50,7 +50,7 @@ static int daemon_task; /* Handle of deamon task / thread */ extern "C" __EXPORT int vcdevtest_main(int argc, char *argv[]); int vcdevtest_main(int argc, char *argv[]) { - + if (argc < 2) { printf("usage: vcdevtest {start|stop|status}\n"); return 1; @@ -65,11 +65,11 @@ int vcdevtest_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("vcdevtest", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/wqueue/CMakeLists.txt b/src/platforms/posix/tests/wqueue/CMakeLists.txt new file mode 100644 index 000000000000..6efd69894964 --- /dev/null +++ b/src/platforms/posix/tests/wqueue/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__tests__wqueue + MAIN wqueue_test + SRCS + wqueue_main.cpp + wqueue_start_posix.cpp + wqueue_test.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp index 2479020097b5..d4652f8e6263 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp @@ -52,7 +52,7 @@ static int daemon_task; /* Handle of deamon task / thread */ extern "C" __EXPORT int wqueue_test_main(int argc, char *argv[]); int wqueue_test_main(int argc, char *argv[]) { - + if (argc < 2) { PX4_INFO("usage: wqueue_test {start|stop|status}\n"); return 1; @@ -67,11 +67,11 @@ int wqueue_test_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("wqueue", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/wqueue/wqueue_test.cpp b/src/platforms/posix/tests/wqueue/wqueue_test.cpp index 6c9ececc143a..aec10269b8bd 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_test.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_test.cpp @@ -65,8 +65,11 @@ void WQueueTest::do_lp_work() { static int iter = 0; printf("done lp work\n"); - if (iter > 5) + + if (iter > 5) { _lpwork_done = true; + } + ++iter; work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1000); @@ -76,8 +79,11 @@ void WQueueTest::do_hp_work() { static int iter = 0; printf("done hp work\n"); - if (iter > 5) + + if (iter > 5) { _hpwork_done = true; + } + ++iter; // requeue diff --git a/src/platforms/posix/tests/wqueue/wqueue_test.h b/src/platforms/posix/tests/wqueue/wqueue_test.h index 6db3fc1e2535..56e66a0b1bb9 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_test.h +++ b/src/platforms/posix/tests/wqueue/wqueue_test.h @@ -43,11 +43,12 @@ #include #include -class WQueueTest { +class WQueueTest +{ public: - WQueueTest() : - _lpwork_done(false), - _hpwork_done(false) + WQueueTest() : + _lpwork_done(false), + _hpwork_done(false) { memset(&_lpwork, 0, sizeof(_lpwork)); memset(&_hpwork, 0, sizeof(_hpwork)); diff --git a/src/platforms/posix/work_queue/CMakeLists.txt b/src/platforms/posix/work_queue/CMakeLists.txt new file mode 100644 index 000000000000..1ee2dec52dfc --- /dev/null +++ b/src/platforms/posix/work_queue/CMakeLists.txt @@ -0,0 +1,55 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__posix__work_queue + COMPILE_FLAGS + -Os + SRCS + hrt_thread.c + hrt_queue.c + hrt_work_cancel.c + work_thread.c + work_lock.c + work_queue.c + work_cancel.c + queue.c + dq_addlast.c + dq_remfirst.c + sq_addlast.c + sq_remfirst.c + sq_addafter.c + dq_rem.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/work_queue/dq_addlast.c b/src/platforms/posix/work_queue/dq_addlast.c new file mode 100644 index 000000000000..35e42d46d3a7 --- /dev/null +++ b/src/platforms/posix/work_queue/dq_addlast.c @@ -0,0 +1,73 @@ +/************************************************************ + * libc/queue/dq_addlast.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_addlast + * + * Description + * dq_addlast adds 'node' to the end of 'queue' + * + ************************************************************/ + +void dq_addlast(dq_entry_t *node, dq_queue_t *queue) +{ + node->flink = NULL; + node->blink = queue->tail; + + if (!queue->head) { + queue->head = node; + queue->tail = node; + + } else { + queue->tail->flink = node; + queue->tail = node; + } +} + diff --git a/src/platforms/posix/work_queue/dq_rem.c b/src/platforms/posix/work_queue/dq_rem.c new file mode 100644 index 000000000000..e6c9bbed3767 --- /dev/null +++ b/src/platforms/posix/work_queue/dq_rem.c @@ -0,0 +1,81 @@ +/************************************************************ + * libc/queue/dq_rem.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_rem + * + * Descripton: + * dq_rem removes 'node' from 'queue' + * + ************************************************************/ + +void dq_rem(dq_entry_t *node, dq_queue_t *queue) +{ + dq_entry_t *prev = node->blink; + dq_entry_t *next = node->flink; + + if (!prev) { + queue->head = next; + + } else { + prev->flink = next; + } + + if (!next) { + queue->tail = prev; + + } else { + next->blink = prev; + } + + node->flink = NULL; + node->blink = NULL; +} + diff --git a/src/platforms/posix/work_queue/dq_remfirst.c b/src/platforms/posix/work_queue/dq_remfirst.c new file mode 100644 index 000000000000..10394d10634b --- /dev/null +++ b/src/platforms/posix/work_queue/dq_remfirst.c @@ -0,0 +1,81 @@ +/************************************************************ + * libc/queue/dq_remfirst.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_remfirst + * + * Description: + * dq_remfirst removes 'node' from the head of 'queue' + * + ************************************************************/ + +dq_entry_t *dq_remfirst(dq_queue_t *queue) +{ + dq_entry_t *ret = queue->head; + + if (ret) { + dq_entry_t *next = ret->flink; + + if (!next) { + queue->head = NULL; + queue->tail = NULL; + + } else { + queue->head = next; + next->blink = NULL; + } + + ret->flink = NULL; + ret->blink = NULL; + } + + return ret; +} + diff --git a/src/platforms/posix/work_queue/hrt_queue.c b/src/platforms/posix/work_queue/hrt_queue.c new file mode 100644 index 000000000000..4238f0733e27 --- /dev/null +++ b/src/platforms/posix/work_queue/hrt_queue.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * libc/wqueue/work_queue.c + * + * Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include "hrt_work.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: hrt_work_queue + * + * Description: + * Queue work to be performed at a later time. All queued work will be + * performed on the worker thread of of execution (not the caller's). + * + * The work structure is allocated by caller, but completely managed by + * the work queue logic. The caller should never modify the contents of + * the work queue structure; the caller should not call work_queue() + * again until either (1) the previous work has been performed and removed + * from the queue, or (2) work_cancel() has been called to cancel the work + * and remove it from the work queue. + * + * Input parameters: + * work - The work structure to queue + * worker - The worker callback to be invoked. The callback will invoked + * on the worker thread of execution. + * arg - The argument that will be passed to the workder callback when + * int is invoked. + * delay - Delay (in microseconds) from the time queue until the worker + * is invoked. Zero means to perform the work immediately. + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay) +{ + struct wqueue_s *wqueue = &g_hrt_work; + + /* First, initialize the work structure */ + + work->worker = worker; /* Work callback */ + work->arg = arg; /* Callback argument */ + work->delay = delay; /* Delay until work performed */ + + /* Now, time-tag that entry and put it in the work queue. This must be + * done with interrupts disabled. This permits this function to be called + * from with task logic or interrupt handlers. + */ + + hrt_work_lock(); + work->qtime = hrt_absolute_time(); /* Time work queued */ + //PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime); + + dq_addlast((dq_entry_t *)work, &wqueue->q); +#ifdef __PX4_QURT + px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ +#else + px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ +#endif + + hrt_work_unlock(); + return PX4_OK; +} + diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c new file mode 100644 index 000000000000..98331ca7981a --- /dev/null +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -0,0 +1,284 @@ +/**************************************************************************** + * libc/wqueue/work_thread.c + * + * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * Modified by: Mark Charlebois to use hrt compatible times + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "hrt_work.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/* The state of each work queue. */ +struct wqueue_s g_hrt_work; + +/**************************************************************************** + * Private Variables + ****************************************************************************/ +px4_sem_t _hrt_work_lock; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ +static void hrt_work_process(void); + +static void _sighandler(int sig_num); + +/**************************************************************************** + * Name: _sighandler + * + * Description: + * This is the handler for the signal to wake the queue processing thread + * + * Input parameters: + * sig_num - the received signal + * + * Returned Value: + * None + * + ****************************************************************************/ +static void _sighandler(int sig_num) +{ + PX4_DEBUG("RECEIVED SIGNAL %d", sig_num); +} + +/**************************************************************************** + * Name: work_process + * + * Description: + * This is the logic that performs actions placed on any work list. + * + * Input parameters: + * wqueue - Describes the work queue to be processed + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void hrt_work_process() +{ + struct wqueue_s *wqueue = &g_hrt_work; + volatile struct work_s *work; + worker_t worker; + void *arg; + uint64_t elapsed; + uint32_t remaining; + uint32_t next; + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + /* Default to sleeping for 1 sec */ + next = 1000000; + + hrt_work_lock(); + + work = (struct work_s *)wqueue->q.head; + + while (work) { + /* Is this work ready? It is ready if there is no delay or if + * the delay has elapsed. qtime is the time that the work was added + * to the work queue. It will always be greater than or equal to + * zero. Therefore a delay of zero will always execute immediately. + */ + + elapsed = hrt_absolute_time() - work->qtime; + + //PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work); + if (elapsed >= work->delay) { + /* Remove the ready-to-execute work from the list */ + + (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); + //PX4_INFO("Dequeued work=%p", work); + + /* Extract the work description from the entry (in case the work + * instance by the re-used after it has been de-queued). + */ + + worker = work->worker; + arg = work->arg; + + /* Mark the work as no longer being queued */ + + work->worker = NULL; + + /* Do the work. Re-enable interrupts while the work is being + * performed... we don't have any idea how long that will take! + */ + + hrt_work_unlock(); + + if (!worker) { + PX4_ERR("MESSED UP: worker = 0"); + PX4_BACKTRACE(); + + } else { + worker(arg); + } + + /* Now, unfortunately, since we re-enabled interrupts we don't + * know the state of the work list and we will have to start + * back at the head of the list. + */ + + hrt_work_lock(); + work = (struct work_s *)wqueue->q.head; + + } else { + /* This one is not ready.. will it be ready before the next + * scheduled wakeup interval? + */ + + /* Here: elapsed < work->delay */ + remaining = work->delay - elapsed; + + //PX4_INFO("remaining=%u delay=%u elapsed=%lu", remaining, work->delay, elapsed); + if (remaining < next) { + /* Yes.. Then schedule to wake up when the work is ready */ + + next = remaining; + } + + /* Then try the next in the list. */ + + work = (struct work_s *)work->dq.flink; + //PX4_INFO("next %u work %p", next, work); + } + } + + /* Wait awhile to check the work list. We will wait here until either + * the time elapses or until we are awakened by a signal. + */ + hrt_work_unlock(); + + /* might sleep less if a signal received and new item was queued */ + //PX4_INFO("Sleeping for %u usec", next); + usleep(next); +} + +/**************************************************************************** + * Name: work_hrtthread + * + * Description: + * This is the worker threads that performs actions placed on the ISR work + * list. + * + * work_hpthread and work_lpthread: These are the kernel mode work queues + * (also build in the flat build). One of these threads also performs + * periodic garbage collection (that is otherwise performed by the idle + * thread if CONFIG_SCHED_WORKQUEUE is not defined). + * + * These worker threads are started by the OS during normal bringup. + * + * All of these entrypoints are referenced by OS internally and should not + * not be accessed by application logic. + * + * Input parameters: + * argc, argv (not used) + * + * Returned Value: + * Does not return + * + ****************************************************************************/ + +static int work_hrtthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) { + /* First, perform garbage collection. This cleans-up memory de-allocations + * that were queued because they could not be freed in that execution + * context (for example, if the memory was freed from an interrupt handler). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + hrt_work_process(); + } + + return PX4_OK; /* To keep some compilers happy */ +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void hrt_work_queue_init(void) +{ + px4_sem_init(&_hrt_work_lock, 0, 1); + + // Create high priority worker thread + g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 2000, + work_hrtthread, + (char *const *)NULL); + + +#ifdef __PX4_QURT + signal(SIGALRM, _sighandler); +#else + signal(SIGCONT, _sighandler); +#endif +} + diff --git a/src/platforms/posix/work_queue/hrt_work_cancel.c b/src/platforms/posix/work_queue/hrt_work_cancel.c new file mode 100644 index 000000000000..d8c5957c0b8b --- /dev/null +++ b/src/platforms/posix/work_queue/hrt_work_cancel.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * libc/wqueue/work_cancel.c + * + * Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include "hrt_work.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: hrt_work_cancel + * + * Description: + * Cancel previously queued work. This removes work from the work queue. + * After work has been canceled, it may be re-queue by calling + * hrt_work_queue() again. + * + * Input parameters: + * work - The previously queue work structure to cancel + * + ****************************************************************************/ + +void hrt_work_cancel(struct work_s *work) +{ + struct wqueue_s *wqueue = &g_hrt_work; + + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + + /* Cancelling the work is simply a matter of removing the work structure + * from the work queue. This must be done with interrupts disabled because + * new work is typically added to the work queue from interrupt handlers. + */ + + hrt_work_lock(); + + if (work->worker != NULL) { + /* A little test of the integrity of the work queue */ + + //DEBUGASSERT(work->dq.flink ||(dq_entry_t *)work == wqueue->q.tail); + //DEBUGASSERT(work->dq.blink ||(dq_entry_t *)work == wqueue->q.head); + + /* Remove the entry from the work queue and make sure that it is + * mark as availalbe (i.e., the worker field is nullified). + */ + + dq_rem((dq_entry_t *)work, &wqueue->q); + work->worker = NULL; + } + + hrt_work_unlock(); +} diff --git a/src/platforms/posix/work_queue/module.mk b/src/platforms/posix/work_queue/module.mk new file mode 100644 index 000000000000..2a493ab7b6f8 --- /dev/null +++ b/src/platforms/posix/work_queue/module.mk @@ -0,0 +1,54 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# POSIX compatible queue and work_queue implementation +# + +SRCS = \ + hrt_thread.c \ + hrt_queue.c \ + hrt_work_cancel.c \ + work_thread.c \ + work_lock.c \ + work_queue.c \ + work_cancel.c \ + queue.c \ + dq_addlast.c \ + dq_remfirst.c \ + sq_addlast.c \ + sq_remfirst.c \ + sq_addafter.c \ + dq_rem.c + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/work_queue/queue.c b/src/platforms/posix/work_queue/queue.c new file mode 100644 index 000000000000..6f9d5b7bde3e --- /dev/null +++ b/src/platforms/posix/work_queue/queue.c @@ -0,0 +1,98 @@ +/************************************************************************ + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + * Modified for use in Linux by Mark Charlebois + * + ************************************************************************/ + +// FIXME - need px4_queue +#include +#include + +__EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue); +sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue); +void sq_rem(sq_entry_t *node, sq_queue_t *queue) +{ + if (queue->head && node) { + if (node == queue->head) { + queue->head = node->flink; + + if (node == queue->tail) { + queue->tail = NULL; + } + + } else { + sq_entry_t *prev; + + for (prev = (sq_entry_t *)queue->head; + prev && prev->flink != node; + prev = prev->flink); + + if (prev) { + sq_remafter(prev, queue); + } + } + } +} + +sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue) +{ + sq_entry_t *ret = node->flink; + + if (queue->head && ret) { + if (queue->tail == ret) { + queue->tail = node; + node->flink = NULL; + + } else { + node->flink = ret->flink; + } + + ret->flink = NULL; + } + + return ret; +} + +__EXPORT void sq_addfirst(sq_entry_t *node, sq_queue_t *queue); +void sq_addfirst(sq_entry_t *node, sq_queue_t *queue) +{ + node->flink = queue->head; + + if (!queue->head) { + queue->tail = node; + } + + queue->head = node; +} + + diff --git a/src/platforms/posix/work_queue/sq_addafter.c b/src/platforms/posix/work_queue/sq_addafter.c new file mode 100644 index 000000000000..5200275253e6 --- /dev/null +++ b/src/platforms/posix/work_queue/sq_addafter.c @@ -0,0 +1,69 @@ +/************************************************************ + * libc/queue/sq_addafter.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_addafter.c + * + * Description: + * The sq_addafter function adds 'node' after 'prev' in the + * 'queue.' + * + ************************************************************/ + +void sq_addafter(sq_entry_t *prev, sq_entry_t *node, + sq_queue_t *queue) +{ + if (!queue->head || prev == queue->tail) { + sq_addlast(node, queue); + + } else { + node->flink = prev->flink; + prev->flink = node; + } +} diff --git a/src/platforms/posix/work_queue/sq_addlast.c b/src/platforms/posix/work_queue/sq_addlast.c new file mode 100644 index 000000000000..1a0ff88a447c --- /dev/null +++ b/src/platforms/posix/work_queue/sq_addlast.c @@ -0,0 +1,72 @@ +/************************************************************ + * libc/queue/sq_addlast.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_addlast + * + * Description: + * The sq_addlast function places the 'node' at the tail of + * the 'queue' + ************************************************************/ + +void sq_addlast(sq_entry_t *node, sq_queue_t *queue) +{ + node->flink = NULL; + + if (!queue->head) { + queue->head = node; + queue->tail = node; + + } else { + queue->tail->flink = node; + queue->tail = node; + } +} + diff --git a/src/platforms/posix/work_queue/sq_remfirst.c b/src/platforms/posix/work_queue/sq_remfirst.c new file mode 100644 index 000000000000..42b0b6599dba --- /dev/null +++ b/src/platforms/posix/work_queue/sq_remfirst.c @@ -0,0 +1,76 @@ +/************************************************************ + * libc/queue/sq_remfirst.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_remfirst + * + * Description: + * sq_remfirst function removes the first entry from + * 'queue' + * + ************************************************************/ + +sq_entry_t *sq_remfirst(sq_queue_t *queue) +{ + sq_entry_t *ret = queue->head; + + if (ret) { + queue->head = ret->flink; + + if (!queue->head) { + queue->tail = NULL; + } + + ret->flink = NULL; + } + + return ret; +} + diff --git a/src/platforms/posix/work_queue/work_cancel.c b/src/platforms/posix/work_queue/work_cancel.c new file mode 100644 index 000000000000..de6a4c3bdcf5 --- /dev/null +++ b/src/platforms/posix/work_queue/work_cancel.c @@ -0,0 +1,120 @@ +/**************************************************************************** + * libc/wqueue/work_cancel.c + * + * Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include "work_lock.h" + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_cancel + * + * Description: + * Cancel previously queued work. This removes work from the work queue. + * After work has been canceled, it may be re-queue by calling work_queue() + * again. + * + * Input parameters: + * qid - The work queue ID + * work - The previously queue work structure to cancel + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int work_cancel(int qid, struct work_s *work) +{ + struct wqueue_s *wqueue = &g_work[qid]; + + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + + /* Cancelling the work is simply a matter of removing the work structure + * from the work queue. This must be done with interrupts disabled because + * new work is typically added to the work queue from interrupt handlers. + */ + + work_lock(qid); + + if (work->worker != NULL) { + /* A little test of the integrity of the work queue */ + + //DEBUGASSERT(work->dq.flink ||(dq_entry_t *)work == wqueue->q.tail); + //DEBUGASSERT(work->dq.blink ||(dq_entry_t *)work == wqueue->q.head); + + /* Remove the entry from the work queue and make sure that it is + * mark as availalbe (i.e., the worker field is nullified). + */ + + dq_rem((dq_entry_t *)work, &wqueue->q); + work->worker = NULL; + } + + work_unlock(qid); + return PX4_OK; +} + +#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/work_queue/work_lock.c b/src/platforms/posix/work_queue/work_lock.c new file mode 100644 index 000000000000..31fa1b7562f3 --- /dev/null +++ b/src/platforms/posix/work_queue/work_lock.c @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#include +#include +#include +#include "work_lock.h" + + +extern px4_sem_t _work_lock[]; + +void work_lock(int id) +{ + px4_sem_wait(&_work_lock[id]); +} + +void work_unlock(int id) +{ + px4_sem_post(&_work_lock[id]); +} diff --git a/src/platforms/posix/work_queue/work_lock.h b/src/platforms/posix/work_queue/work_lock.h new file mode 100644 index 000000000000..ad2e5b4a01e1 --- /dev/null +++ b/src/platforms/posix/work_queue/work_lock.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ + +#ifndef _work_lock_h_ +#define _work_lock_h_ + + +//#pragma once + +void work_lock(int id); +void work_unlock(int id); + +#endif // _work_lock_h_ diff --git a/src/platforms/posix/work_queue/work_queue.c b/src/platforms/posix/work_queue/work_queue.c new file mode 100644 index 000000000000..36d07ca3d9bf --- /dev/null +++ b/src/platforms/posix/work_queue/work_queue.c @@ -0,0 +1,137 @@ +/**************************************************************************** + * libc/wqueue/work_queue.c + * + * Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include "work_lock.h" + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_queue + * + * Description: + * Queue work to be performed at a later time. All queued work will be + * performed on the worker thread of of execution (not the caller's). + * + * The work structure is allocated by caller, but completely managed by + * the work queue logic. The caller should never modify the contents of + * the work queue structure; the caller should not call work_queue() + * again until either (1) the previous work has been performed and removed + * from the queue, or (2) work_cancel() has been called to cancel the work + * and remove it from the work queue. + * + * Input parameters: + * qid - The work queue ID (index) + * work - The work structure to queue + * worker - The worker callback to be invoked. The callback will invoked + * on the worker thread of execution. + * arg - The argument that will be passed to the workder callback when + * int is invoked. + * delay - Delay (in clock ticks) from the time queue until the worker + * is invoked. Zero means to perform the work immediately. + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) +{ + struct wqueue_s *wqueue = &g_work[qid]; + + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + + /* First, initialize the work structure */ + + work->worker = worker; /* Work callback */ + work->arg = arg; /* Callback argument */ + work->delay = delay; /* Delay until work performed */ + + /* Now, time-tag that entry and put it in the work queue. This must be + * done with interrupts disabled. This permits this function to be called + * from with task logic or interrupt handlers. + */ + + work_lock(qid); + work->qtime = clock_systimer(); /* Time work queued */ + + dq_addlast((dq_entry_t *)work, &wqueue->q); +#ifdef __PX4_QURT + px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ +#else + px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ +#endif + + work_unlock(qid); + return PX4_OK; +} + +#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/work_queue/work_thread.c b/src/platforms/posix/work_queue/work_thread.c new file mode 100644 index 000000000000..ec565bcfcc17 --- /dev/null +++ b/src/platforms/posix/work_queue/work_thread.c @@ -0,0 +1,327 @@ +/**************************************************************************** + * libc/wqueue/work_thread.c + * + * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "work_lock.h" + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/* The state of each work queue. */ +struct wqueue_s g_work[NWORKERS]; + +/**************************************************************************** + * Private Variables + ****************************************************************************/ +px4_sem_t _work_lock[NWORKERS]; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_process + * + * Description: + * This is the logic that performs actions placed on any work list. + * + * Input parameters: + * wqueue - Describes the work queue to be processed + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void work_process(struct wqueue_s *wqueue, int lock_id) +{ + volatile struct work_s *work; + worker_t worker; + void *arg; + uint64_t elapsed; + uint32_t remaining; + uint32_t next; + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + next = CONFIG_SCHED_WORKPERIOD; + + work_lock(lock_id); + + work = (struct work_s *)wqueue->q.head; + + while (work) { + /* Is this work ready? It is ready if there is no delay or if + * the delay has elapsed. qtime is the time that the work was added + * to the work queue. It will always be greater than or equal to + * zero. Therefore a delay of zero will always execute immediately. + */ + + elapsed = USEC2TICK(clock_systimer() - work->qtime); + + //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay); + if (elapsed >= work->delay) { + /* Remove the ready-to-execute work from the list */ + + (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); + + /* Extract the work description from the entry (in case the work + * instance by the re-used after it has been de-queued). + */ + + worker = work->worker; + arg = work->arg; + + /* Mark the work as no longer being queued */ + + work->worker = NULL; + + /* Do the work. Re-enable interrupts while the work is being + * performed... we don't have any idea how long that will take! + */ + + work_unlock(lock_id); + + if (!worker) { + PX4_WARN("MESSED UP: worker = 0\n"); + + } else { + worker(arg); + } + + /* Now, unfortunately, since we re-enabled interrupts we don't + * know the state of the work list and we will have to start + * back at the head of the list. + */ + + work_lock(lock_id); + work = (struct work_s *)wqueue->q.head; + + } else { + /* This one is not ready.. will it be ready before the next + * scheduled wakeup interval? + */ + + /* Here: elapsed < work->delay */ + remaining = USEC_PER_TICK * (work->delay - elapsed); + + if (remaining < next) { + /* Yes.. Then schedule to wake up when the work is ready */ + + next = remaining; + } + + /* Then try the next in the list. */ + + work = (struct work_s *)work->dq.flink; + } + } + + /* Wait awhile to check the work list. We will wait here until either + * the time elapses or until we are awakened by a signal. + */ + work_unlock(lock_id); + + usleep(next); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +void work_queues_init(void) +{ + px4_sem_init(&_work_lock[HPWORK], 0, 1); + px4_sem_init(&_work_lock[LPWORK], 0, 1); +#ifdef CONFIG_SCHED_USRWORK + px4_sem_init(&_work_lock[USRWORK], 0, 1); +#endif + + // Create high priority worker thread + g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 1, + 2000, + work_hpthread, + (char *const *)NULL); + + // Create low priority worker thread + g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", + SCHED_DEFAULT, + SCHED_PRIORITY_MIN, + 2000, + work_lpthread, + (char *const *)NULL); + +} + +/**************************************************************************** + * Name: work_hpthread, work_lpthread, and work_usrthread + * + * Description: + * These are the worker threads that performs actions placed on the work + * lists. + * + * work_hpthread and work_lpthread: These are the kernel mode work queues + * (also build in the flat build). One of these threads also performs + * periodic garbage collection (that is otherwise performed by the idle + * thread if CONFIG_SCHED_WORKQUEUE is not defined). + * + * These worker threads are started by the OS during normal bringup. + * + * work_usrthread: This is a user mode work queue. It must be built into + * the applicatino blob during the user phase of a kernel build. The + * user work thread will then automatically be started when the system + * boots by calling through the pointer found in the header on the user + * space blob. + * + * All of these entrypoints are referenced by OS internally and should not + * not be accessed by application logic. + * + * Input parameters: + * argc, argv (not used) + * + * Returned Value: + * Does not return + * + ****************************************************************************/ + +#ifdef CONFIG_SCHED_HPWORK + +int work_hpthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) { + /* First, perform garbage collection. This cleans-up memory de-allocations + * that were queued because they could not be freed in that execution + * context (for example, if the memory was freed from an interrupt handler). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ + +#ifndef CONFIG_SCHED_LPWORK + sched_garbagecollection(); +#endif + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + work_process(&g_work[HPWORK], HPWORK); + } + + return PX4_OK; /* To keep some compilers happy */ +} + +#ifdef CONFIG_SCHED_LPWORK + +int work_lpthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) { + /* First, perform garbage collection. This cleans-up memory de-allocations + * that were queued because they could not be freed in that execution + * context (for example, if the memory was freed from an interrupt handler). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ + + //sched_garbagecollection(); + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + work_process(&g_work[LPWORK], LPWORK); + } + + return PX4_OK; /* To keep some compilers happy */ +} + +#endif /* CONFIG_SCHED_LPWORK */ +#endif /* CONFIG_SCHED_HPWORK */ + +#ifdef CONFIG_SCHED_USRWORK + +int work_usrthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) { + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + work_process(&g_work[USRWORK], USRWORK); + } + + return PX4_OK; /* To keep some compilers happy */ +} + +#endif /* CONFIG_SCHED_USRWORK */ + +uint32_t clock_systimer() +{ + //printf("clock_systimer: %0lx\n", hrt_absolute_time()); + return (0x00000000ffffffff & hrt_absolute_time()); +} +#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/px4_adc.h b/src/platforms/px4_adc.h index 8420f519715c..c8c8ce01f0b6 100644 --- a/src/platforms/px4_adc.h +++ b/src/platforms/px4_adc.h @@ -52,13 +52,12 @@ // FIXME - this needs to be a px4_adc_msg_s type // Curently copied from NuttX -struct adc_msg_s -{ - uint8_t am_channel; /* The 8-bit ADC Channel */ - int32_t am_data; /* ADC convert result (4 bytes) */ -} __attribute__ ((packed)); +struct adc_msg_s { + uint8_t am_channel; /* The 8-bit ADC Channel */ + int32_t am_data; /* ADC convert result (4 bytes) */ +} __attribute__((packed)); -// Example settings +// Example settings #define ADC_BATTERY_VOLTAGE_CHANNEL 10 #define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 diff --git a/src/platforms/px4_app.h b/src/platforms/px4_app.h index 9aa285e228c3..df3a8d8294e9 100644 --- a/src/platforms/px4_app.h +++ b/src/platforms/px4_app.h @@ -40,9 +40,11 @@ #pragma once -namespace px4 { +namespace px4 +{ -class AppState { +class AppState +{ public: ~AppState() {} @@ -65,8 +67,8 @@ class AppState { bool _isRunning; #endif private: - AppState(const AppState&); - const AppState& operator=(const AppState&); + AppState(const AppState &); + const AppState &operator=(const AppState &); }; } diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index e72441108003..4417c5b894ba 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -49,7 +49,7 @@ #define CONFIG_SCHED_WORKQUEUE 1 #define CONFIG_SCHED_HPWORK 1 #define CONFIG_SCHED_LPWORK 1 -#define CONFIG_ARCH_BOARD_POSIXTEST 1 +#define CONFIG_ARCH_BOARD_SITL 1 /** time in ms between checks for work in work queues **/ #define CONFIG_SCHED_WORKPERIOD 50000 diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index db54f8c53ec5..4d453e59830f 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -92,6 +92,8 @@ typedef param_t px4_param_t; */ #if defined(__PX4_NUTTX) +#define PX4_ROOTFSDIR + /* XXX this is a hack to resolve conflicts with NuttX headers */ #if !defined(__PX4_TESTS) #define isspace(c) \ @@ -105,11 +107,19 @@ typedef param_t px4_param_t; #define PX4_ISFINITE(x) isfinite(x) +// mode for open with O_CREAT +#define PX4_O_MODE_777 0777 +#define PX4_O_MODE_666 0666 +#define PX4_O_MODE_600 0600 + #ifndef PRIu64 #define PRIu64 "llu" #endif +#ifndef PRId64 +#define PRId64 "lld" +#endif -/* +/* * POSIX Specific defines */ #elif defined(__PX4_POSIX) @@ -117,35 +127,54 @@ typedef param_t px4_param_t; // Flag is meaningless on Linux #define O_BINARY 0 +// mode for open with O_CREAT +#define PX4_O_MODE_777 (S_IRWXU | S_IRWXG | S_IRWXO) +#define PX4_O_MODE_666 (S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH ) +#define PX4_O_MODE_600 (S_IRUSR | S_IWUSR) + + // NuttX _IOC is equivalent to Linux _IO #define _PX4_IOC(x,y) _IO(x,y) /* FIXME - Used to satisfy build */ //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) -#define UNIQUE_ID 0x1FFF7A10 +#define UNIQUE_ID 0x1FFF7A10 +#define STM32_SYSMEM_UID "SIMULATIONID" /* FIXME - Used to satisfy build */ #define getreg32(a) (*(volatile uint32_t *)(a)) +#ifdef __PX4_QURT +#define PX4_TICKS_PER_SEC 1000L +#else __BEGIN_DECLS extern long PX4_TICKS_PER_SEC; __END_DECLS +#endif -#define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)(x)/1000000L) -#define USEC_PER_TICK (1000000L/PX4_TICKS_PER_SEC) +#define USEC_PER_TICK (1000000UL/PX4_TICKS_PER_SEC) +#define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK) #define px4_statfs_buf_f_bavail_t unsigned long +#if defined(__PX4_QURT) +#define PX4_ROOTFSDIR +#else +#define PX4_ROOTFSDIR "rootfs" #endif +#endif -/* + +/* * Defines for ROS and Linux */ #if defined(__PX4_ROS) || defined(__PX4_POSIX) #define OK 0 #define ERROR -1 +#define MAX_RAND 32767 + #if defined(__PX4_QURT) #define M_PI 3.14159265358979323846 #define M_PI_2 1.57079632679489661923 @@ -187,6 +216,7 @@ __END_DECLS #ifndef __PX4_QURT #if defined(__cplusplus) +#include #define PX4_ISFINITE(x) std::isfinite(x) #else #define PX4_ISFINITE(x) isfinite(x) @@ -196,22 +226,18 @@ __END_DECLS #endif #if defined(__PX4_QURT) + +#define PX4_ROOTFSDIR +#define DEFAULT_PARAM_FILE "/fs/eeprom/parameters" + #define SIOCDEVPRIVATE 999999 // Missing math.h defines -#define PX4_ISFINITE(x) isfinite(x) +#define PX4_ISFINITE(x) __builtin_isfinite(x) -// FIXME - these are missing for clang++ but not for clang -#if defined(__cplusplus) -#define isfinite(x) true -#define isnan(x) false -#define isinf(x) false -#define fminf(x, y) ((x) > (y) ? y : x) #endif -#endif - -/* +/* *Defines for all platforms */ diff --git a/src/platforms/px4_i2c.h b/src/platforms/px4_i2c.h index 2b72f37c5deb..1d496004730b 100644 --- a/src/platforms/px4_i2c.h +++ b/src/platforms/px4_i2c.h @@ -75,23 +75,23 @@ typedef struct i2c_dev_s px4_i2c_dev_t; // NOTE - This is a copy of the NuttX i2c_msg_s structure typedef struct { - uint16_t addr; /* Slave address */ - uint16_t flags; /* See I2C_M_* definitions */ - uint8_t *buffer; - int length; + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; } px4_i2c_msg_t; // NOTE - This is a copy of the NuttX i2c_ops_s structure typedef struct { - const struct px4_i2c_ops_t *ops; /* I2C vtable */ + const struct px4_i2c_ops_t *ops; /* I2C vtable */ } px4_i2c_dev_t; // FIXME - Empty defines for I2C ops // Original version commented out //#define I2C_SETFREQUENCY(d,f) ((d)->ops->setfrequency(d,f)) -#define I2C_SETFREQUENCY(d,f) +#define I2C_SETFREQUENCY(d,f) //#define SPI_SELECT(d,id,s) ((d)->ops->select(d,id,s)) -#define SPI_SELECT(d,id,s) +#define SPI_SELECT(d,id,s) // FIXME - Stub implementation // Original version commented out @@ -101,8 +101,7 @@ inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count) { ret #ifdef __PX4_QURT -struct i2c_msg -{ +struct i2c_msg { uint16_t addr; /* Slave address */ uint16_t flags; /* See I2C_M_* definitions */ uint8_t *buf; diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 83bb60d79660..0286705968b8 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -87,6 +88,7 @@ #include #include #include +#include #include #include #include @@ -120,6 +122,7 @@ #include #include #include +#include #include #include #include @@ -150,6 +153,7 @@ #include #include #include +#include #include #include #include diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index a0d7f07bceac..953b766f4d8d 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -33,63 +33,350 @@ /** * @file px4_log.h - * Platform dependant logging/debug + * Platform dependant logging/debug implementation */ #pragma once -#define __px4_log_omit(level, ...) { } +#define _PX4_LOG_LEVEL_ALWAYS 0 +#define _PX4_LOG_LEVEL_DEBUG 1 +#define _PX4_LOG_LEVEL_WARN 2 +#define _PX4_LOG_LEVEL_ERROR 3 +#define _PX4_LOG_LEVEL_PANIC 4 -#define __px4_log(level, ...) { \ - printf("%-5s ", level);\ - printf(__VA_ARGS__);\ - printf("\n");\ +// Used to silence unused variable warning +static inline void do_nothing(int level, ...) +{ + (void)level; } -#define __px4_log_verbose(level, ...) { \ - printf("%-5s ", level);\ - printf(__VA_ARGS__);\ - printf(" (file %s line %d)\n", __FILE__, __LINE__);\ -} -#if defined(__PX4_QURT) -#include -#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__); -#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__); -#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__); -#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__); +/**************************************************************************** + * __px4_log_omit: + * Compile out the message + ****************************************************************************/ +#define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__) -#elif defined(__PX4_LINUX) -#include -#include +#if defined(__PX4_ROS) -#define __px4_log_threads(level, ...) { \ - printf("%-5s %ld ", level, pthread_self());\ - printf(__VA_ARGS__);\ - printf(" (file %s line %d)\n", __FILE__, __LINE__);\ -} +#include +#define PX4_PANIC(...) ROS_WARN(__VA_ARGS__) +#define PX4_ERR(...) ROS_WARN(__VA_ARGS__) +#define PX4_WARN(...) ROS_WARN(__VA_ARGS__) +#define PX4_INFO(...) ROS_WARN(__VA_ARGS__) +#define PX4_DEBUG(...) +#define PX4_BACKTRACE() -#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__); -#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__); -#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__); -#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__); +#elif defined(__PX4_QURT) +#include "qurt_log.h" +/**************************************************************************** + * Messages that should never be filtered or compiled out + ****************************************************************************/ +#define PX4_LOG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_INFO(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_BACKTRACE() -#elif defined(__PX4_ROS) +#if defined(TRACE_BUILD) +/**************************************************************************** + * Extremely Verbose settings for a Trace build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILE__, __LINE__, FMT, ##__VA_ARGS__) -#define PX4_DBG(...) -#define PX4_INFO(...) ROS_WARN(__VA_ARGS__) -#define PX4_WARN(...) ROS_WARN(__VA_ARGS__) -#define PX4_ERR(...) ROS_WARN(__VA_ARGS__) +#elif defined(DEBUG_BUILD) +/**************************************************************************** + * Verbose settings for a Debug build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILE__, __LINE__, FMT, ##__VA_ARGS__) + +#elif defined(RELEASE_BUILD) +/**************************************************************************** + * Non-verbose settings for a Release build to minimize strings in build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) -#elif defined(__PX4_NUTTX) -#include +#else +/**************************************************************************** + * Medium verbose settings for a default build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) -#define PX4_DBG(...) -#define PX4_INFO(...) warnx(__VA_ARGS__) -#define PX4_WARN(...) warnx(__VA_ARGS__) -#define PX4_ERR(...) warnx(__VA_ARGS__) +#endif +#define PX4_LOG_NAMED(name, FMT, ...) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) +#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) if( cond ) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) #else -#error "Target platform unknown" +#define __STDC_FORMAT_MACROS +#include +#include +#include +#include +#include + +__BEGIN_DECLS +__EXPORT extern uint64_t hrt_absolute_time(void); + +__EXPORT extern const char *__px4_log_level_str[5]; +__EXPORT extern int __px4_log_level_current; +__EXPORT extern void px4_backtrace(void); +__END_DECLS + +#define PX4_BACKTRACE() px4_backtrace() + +// __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME +#define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_ERROR + +/**************************************************************************** + * Implementation of log section formatting based on printf + * + * To write to a specific stream for each message type, open the streams and + * set __px4__log_startline to something like: + * if (level <= __px4_log_level_current) printf(_px4_fd[level], + * + * Additional behavior can be added using "{\" for __px4__log_startline and + * "}" for __px4__log_endline and any other required setup or teardown steps + ****************************************************************************/ +#define __px4__log_printcond(cond, ...) if (cond) printf(__VA_ARGS__) +#define __px4__log_printline(level, ...) if (level <= __px4_log_level_current) printf(__VA_ARGS__) + +#define __px4__log_timestamp_fmt "%-10" PRIu64 " " +#define __px4__log_timestamp_arg ,hrt_absolute_time() +#define __px4__log_level_fmt "%-5s " +#define __px4__log_level_arg(level) ,__px4_log_level_str[level] +#define __px4__log_thread_fmt "%#X " +#define __px4__log_thread_arg ,(unsigned int)pthread_self() + +#define __px4__log_file_and_line_fmt " (file %s line %u)" +#define __px4__log_file_and_line_arg , __FILE__, __LINE__ +#define __px4__log_end_fmt "\n" +#define __px4__log_endline ) + +/**************************************************************************** + * Output format macros + * Use these to implement the code level macros below + ****************************************************************************/ + + +/**************************************************************************** + * __px4_log_named_cond: + * Convert a message in the form: + * PX4_LOG_COND(__dbg_enabled, "val is %d", val); + * to + * printf("%-5s val is %d\n", "LOG", val); + * if the first arg/condition is true. + ****************************************************************************/ +#define __px4_log_named_cond(name, cond, FMT, ...) \ + __px4__log_printcond(cond,\ + "%s " \ + FMT\ + __px4__log_end_fmt \ + ,name, ##__VA_ARGS__\ + ) + +/**************************************************************************** + * __px4_log: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s val is %d\n", __px4_log_level_str[3], val); + ****************************************************************************/ +#define __px4_log(level, FMT, ...) \ + __px4__log_printline(level,\ + __px4__log_level_fmt \ + FMT\ + __px4__log_end_fmt \ + __px4__log_level_arg(level), ##__VA_ARGS__\ + ) + +/**************************************************************************** + * __px4_log_timestamp: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s %10lu val is %d\n", __px4_log_level_str[3], + * hrt_absolute_time(), val); + ****************************************************************************/ +#define __px4_log_timestamp(level, FMT, ...) \ + __px4__log_printline(level,\ + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + FMT\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + , ##__VA_ARGS__\ + ) + +/**************************************************************************** + * __px4_log_timestamp_thread: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s %10lu %#X val is %d\n", __px4_log_level_str[3], + * hrt_absolute_time(), pthread_self(), val); + ****************************************************************************/ +#define __px4_log_timestamp_thread(level, FMT, ...) \ + __px4__log_printline(level,\ + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + __px4__log_thread_fmt\ + FMT\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + __px4__log_thread_arg\ + , ##__VA_ARGS__\ + ) + +/**************************************************************************** + * __px4_log_file_and_line: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s val is %d (file %s line %u)\n", + * __px4_log_level_str[3], val, __FILE__, __LINE__); + ****************************************************************************/ +#define __px4_log_file_and_line(level, FMT, ...) \ + __px4__log_printline(level,\ + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) + +/**************************************************************************** + * __px4_log_timestamp_file_and_line: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s %-10lu val is %d (file %s line %u)\n", + * __px4_log_level_str[3], hrt_absolute_time(), + * val, __FILE__, __LINE__); + ****************************************************************************/ +#define __px4_log_timestamp_file_and_line(level, FMT, ...) \ + __px4__log_printline(level,\ + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) + +/**************************************************************************** + * __px4_log_thread_file_and_line: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s %#X val is %d (file %s line %u)\n", + * __px4_log_level_str[3], pthread_self(), + * val, __FILE__, __LINE__); + ****************************************************************************/ +#define __px4_log_thread_file_and_line(level, FMT, ...) \ + __px4__log_printline(level,\ + __px4__log_level_fmt\ + __px4__log_thread_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_thread_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) + +/**************************************************************************** + * __px4_log_timestamp_thread_file_and_line: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s %-10lu %#X val is %d (file %s line %u)\n", + * __px4_log_level_str[3], hrt_absolute_time(), + * pthread_self(), val, __FILE__, __LINE__); + ****************************************************************************/ +#define __px4_log_timestamp_thread_file_and_line(level, FMT, ...) \ + __px4__log_printline(level,\ + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + __px4__log_thread_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + __px4__log_thread_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) + +/**************************************************************************** + * Code level macros + * These are the log APIs that should be used by the code + ****************************************************************************/ + +/**************************************************************************** + * Messages that should never be filtered or compiled out + ****************************************************************************/ +#define PX4_LOG(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__) +#define PX4_INFO(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__) + +#if defined(TRACE_BUILD) +/**************************************************************************** + * Extremely Verbose settings for a Trace build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) __px4_log_timestamp_thread_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log_timestamp_thread_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_timestamp_thread_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_timestamp_thread(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) + +#elif defined(DEBUG_BUILD) +/**************************************************************************** + * Verbose settings for a Debug build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_timestamp(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) + +#elif defined(RELEASE_BUILD) +/**************************************************************************** + * Non-verbose settings for a Release build to minimize strings in build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) + +#else +/**************************************************************************** + * Medium verbose settings for a default build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) + +#endif +#define PX4_LOG_NAMED(name, FMT, ...) __px4_log_named_cond(name, true, FMT, ##__VA_ARGS__) +#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) __px4_log_named_cond(name, cond, FMT, ##__VA_ARGS__) #endif diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index b0deef16c169..dc806aa905ae 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -48,6 +48,7 @@ /* includes when building for ros */ #include "ros/ros.h" #include +#define __STDC_FORMAT_MACROS #include #include #else @@ -85,7 +86,7 @@ class NodeHandle : template Subscriber *subscribe(void(*fp)(const T &), unsigned interval) { - SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1)); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle *)this, std::bind(fp, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber *)sub; } @@ -98,7 +99,7 @@ class NodeHandle : template Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) { - SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1)); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle *)this, std::bind(fp, obj, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber *)sub; } @@ -109,7 +110,7 @@ class NodeHandle : template Subscriber *subscribe(unsigned interval) { - SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle *)this); _subs.push_back(sub); return (Subscriber *)sub; } @@ -118,11 +119,11 @@ class NodeHandle : * Advertise topic */ template - Publisher* advertise() + Publisher *advertise() { - PublisherROS *pub = new PublisherROS((ros::NodeHandle*)this); - _pubs.push_back((PublisherBase*)pub); - return (Publisher*)pub; + PublisherROS *pub = new PublisherROS((ros::NodeHandle *)this); + _pubs.push_back((PublisherBase *)pub); + return (Publisher *)pub; } /** @@ -241,7 +242,7 @@ class __EXPORT NodeHandle { PublisherUORB *pub = new PublisherUORB(); _pubs.add(pub); - return (Publisher*)pub; + return (Publisher *)pub; } /** diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 67c9a479633f..ab53d03900e6 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -48,6 +48,44 @@ #include +/* Semaphore handling */ + +#ifdef __PX4_DARWIN + +__BEGIN_DECLS + +typedef struct { + pthread_mutex_t lock; + pthread_cond_t wait; + int value; +} px4_sem_t; + +__EXPORT int px4_sem_init(px4_sem_t *s, int pshared, unsigned value); +__EXPORT int px4_sem_wait(px4_sem_t *s); +__EXPORT int px4_sem_post(px4_sem_t *s); +__EXPORT int px4_sem_getvalue(px4_sem_t *s, int *sval); +__EXPORT int px4_sem_destroy(px4_sem_t *s); + +__END_DECLS + +#else + +__BEGIN_DECLS + +typedef sem_t px4_sem_t; + +#define px4_sem_init sem_init +#define px4_sem_wait sem_wait +#define px4_sem_post sem_post +#define px4_sem_getvalue sem_getvalue +#define px4_sem_destroy sem_destroy + +__END_DECLS + +#endif + +//################################### + #ifdef __PX4_NUTTX #define PX4_F_RDONLY 1 @@ -58,7 +96,7 @@ typedef struct pollfd px4_pollfd_struct_t; #if defined(__cplusplus) #define _GLOBAL :: #else -#define _GLOBAL +#define _GLOBAL #endif #define px4_open _GLOBAL open #define px4_close _GLOBAL close @@ -67,6 +105,8 @@ typedef struct pollfd px4_pollfd_struct_t; #define px4_read _GLOBAL read #define px4_poll _GLOBAL poll #define px4_fsync _GLOBAL fsync +#define px4_access _GLOBAL access +#define px4_getpid _GLOBAL getpid #elif defined(__PX4_POSIX) @@ -77,14 +117,14 @@ typedef struct pollfd px4_pollfd_struct_t; typedef short pollevent_t; typedef struct { - /* This part of the struct is POSIX-like */ - int fd; /* The descriptor being polled */ - pollevent_t events; /* The input event flags */ - pollevent_t revents; /* The output event flags */ - - /* Required for PX4 compatability */ - sem_t *sem; /* Pointer to semaphore used to post output event */ - void *priv; /* For use by drivers */ + /* This part of the struct is POSIX-like */ + int fd; /* The descriptor being polled */ + pollevent_t events; /* The input event flags */ + pollevent_t revents; /* The output event flags */ + + /* Required for PX4 compatability */ + px4_sem_t *sem; /* Pointer to semaphore used to post output event */ + void *priv; /* For use by drivers */ } px4_pollfd_struct_t; __BEGIN_DECLS @@ -96,6 +136,8 @@ __EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen); __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); __EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); __EXPORT int px4_fsync(int fd); +__EXPORT int px4_access(const char *pathname, int mode); +__EXPORT unsigned long px4_getpid(void); __END_DECLS #else @@ -107,8 +149,8 @@ extern int px4_errno; __EXPORT void px4_show_devices(void); __EXPORT void px4_show_files(void); -__EXPORT const char * px4_get_device_names(unsigned int *handle); +__EXPORT const char *px4_get_device_names(unsigned int *handle); __EXPORT void px4_show_topics(void); -__EXPORT const char * px4_get_topic_names(unsigned int *handle); +__EXPORT const char *px4_get_topic_names(unsigned int *handle); __END_DECLS diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index eb8e964e7906..fb9ec50e10ea 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -86,7 +86,8 @@ class PublisherROS : */ PublisherROS(ros::NodeHandle *rnh) : Publisher(), - _ros_pub(rnh->advertisedata())>::type &>(T::handle(), kQueueSizeDefault)) + _ros_pub(rnh->advertise < typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type & > (T::handle(), + kQueueSizeDefault)) {} ~PublisherROS() {}; @@ -136,7 +137,8 @@ class __EXPORT PublisherUORB : _uorb_pub(new uORB::PublicationBase(T::handle())) {} - ~PublisherUORB() { + ~PublisherUORB() + { delete _uorb_pub; }; @@ -145,7 +147,7 @@ class __EXPORT PublisherUORB : */ int publish(const T &msg) { - _uorb_pub->update((void *)&(msg.data())); + _uorb_pub->update((void *) & (msg.data())); return 0; } @@ -154,7 +156,7 @@ class __EXPORT PublisherUORB : */ void update() {} ; private: - uORB::PublicationBase * _uorb_pub; /**< Handle to the publisher */ + uORB::PublicationBase *_uorb_pub; /**< Handle to the publisher */ }; #endif diff --git a/src/platforms/px4_spi.h b/src/platforms/px4_spi.h index 17397ee7ac0d..78b747b775d0 100644 --- a/src/platforms/px4_spi.h +++ b/src/platforms/px4_spi.h @@ -3,32 +3,29 @@ #ifdef __PX4_NUTTX #include #elif defined(__PX4_POSIX) -enum spi_dev_e -{ - SPIDEV_NONE = 0, /* Not a valid value */ - SPIDEV_MMCSD, /* Select SPI MMC/SD device */ - SPIDEV_FLASH, /* Select SPI FLASH device */ - SPIDEV_ETHERNET, /* Select SPI ethernet device */ - SPIDEV_DISPLAY, /* Select SPI LCD/OLED display device */ - SPIDEV_WIRELESS, /* Select SPI Wireless device */ - SPIDEV_TOUCHSCREEN, /* Select SPI touchscreen device */ - SPIDEV_EXPANDER, /* Select SPI I/O expander device */ - SPIDEV_MUX, /* Select SPI multiplexer device */ - SPIDEV_AUDIO_DATA, /* Select SPI audio codec device data port */ - SPIDEV_AUDIO_CTRL, /* Select SPI audio codec device control port */ +enum spi_dev_e { + SPIDEV_NONE = 0, /* Not a valid value */ + SPIDEV_MMCSD, /* Select SPI MMC/SD device */ + SPIDEV_FLASH, /* Select SPI FLASH device */ + SPIDEV_ETHERNET, /* Select SPI ethernet device */ + SPIDEV_DISPLAY, /* Select SPI LCD/OLED display device */ + SPIDEV_WIRELESS, /* Select SPI Wireless device */ + SPIDEV_TOUCHSCREEN, /* Select SPI touchscreen device */ + SPIDEV_EXPANDER, /* Select SPI I/O expander device */ + SPIDEV_MUX, /* Select SPI multiplexer device */ + SPIDEV_AUDIO_DATA, /* Select SPI audio codec device data port */ + SPIDEV_AUDIO_CTRL, /* Select SPI audio codec device control port */ }; /* Certain SPI devices may required differnt clocking modes */ -enum spi_mode_e -{ - SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ - SPIDEV_MODE1, /* CPOL=0 CHPHA=1 */ - SPIDEV_MODE2, /* CPOL=1 CHPHA=0 */ - SPIDEV_MODE3 /* CPOL=1 CHPHA=1 */ +enum spi_mode_e { + SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ + SPIDEV_MODE1, /* CPOL=0 CHPHA=1 */ + SPIDEV_MODE2, /* CPOL=1 CHPHA=0 */ + SPIDEV_MODE3 /* CPOL=1 CHPHA=1 */ }; -struct spi_dev_s -{ +struct spi_dev_s { int unused; }; #else diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 9b0ca1319aef..59cfa5a191b9 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -38,8 +38,10 @@ */ #pragma once +#ifndef CONFIG_ARCH_BOARD_SIM #include #include +#endif #if defined(__PX4_ROS) /* includes when building for ros */ @@ -84,12 +86,12 @@ class __EXPORT Subscriber : /** * Get the last message value */ - virtual T& get() {return _msg_current;} + virtual T &get() {return _msg_current;} /** * Get the last native message value */ - virtual decltype(((T*)nullptr)->data()) data() + virtual decltype(((T *)nullptr)->data()) data() { return _msg_current.data(); } @@ -133,7 +135,7 @@ class SubscriberROS : * Called on topic update, saves the current message and then calls the provided callback function * needs to use the native type as it is called by ROS */ - void callback(const typename std::remove_referencedata())>::type &msg) + void callback(const typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type &msg) { /* Store data */ this->_msg_current.data() = msg; @@ -195,7 +197,8 @@ class __EXPORT SubscriberUORB : _uorb_sub(new uORB::SubscriptionBase(T::handle(), interval)) {} - virtual ~SubscriberUORB() { + virtual ~SubscriberUORB() + { delete _uorb_sub; }; @@ -217,17 +220,19 @@ class __EXPORT SubscriberUORB : int getUORBHandle() { return _uorb_sub->getHandle(); } protected: - uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + uORB::SubscriptionBase *_uorb_sub; /**< Handle to the subscription */ - typename std::remove_referencedata())>::type getUORBData() +#ifndef CONFIG_ARCH_BOARD_SIM + typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type getUORBData() { - return (typename std::remove_referencedata())>::type)*_uorb_sub; + return (typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type) * _uorb_sub; } +#endif /** * Get void pointer to last message value */ - void *get_void_ptr() { return (void *)&(this->_msg_current.data()); } + void *get_void_ptr() { return (void *) & (this->_msg_current.data()); } }; @@ -242,8 +247,13 @@ class __EXPORT SubscriberUORBCallback : * @param cbf Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback */ - SubscriberUORBCallback(unsigned interval, - std::function cbf) : + SubscriberUORBCallback(unsigned interval +#ifndef CONFIG_ARCH_BOARD_SIM + , std::function cbf) +#else + ) +#endif + : SubscriberUORB(interval), _cbf(cbf) {} @@ -277,7 +287,9 @@ class __EXPORT SubscriberUORBCallback : }; protected: +#ifndef CONFIG_ARCH_BOARD_SIM std::function _cbf; /**< Callback that the user provided on the subscription */ +#endif }; #endif diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index e7eda424bfce..04585aacedcc 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -67,11 +67,23 @@ typedef int px4_task_t; #ifdef __PX4_LINUX #define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) #define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO) -#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) +#define SCHED_PRIORITY_DEFAULT (((sched_get_priority_max(SCHED_FIFO) - sched_get_priority_min(SCHED_FIFO)) / 2) + sched_get_priority_min(SCHED_FIFO)) +#elif defined(__PX4_DARWIN) +#define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) +#define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO) +#define SCHED_PRIORITY_DEFAULT (((sched_get_priority_max(SCHED_FIFO) - sched_get_priority_min(SCHED_FIFO)) / 2) + sched_get_priority_min(SCHED_FIFO)) #elif defined(__PX4_QURT) #define SCHED_PRIORITY_MAX 0 #define SCHED_PRIORITY_MIN 0 #define SCHED_PRIORITY_DEFAULT 0 +#else +#error "No target OS defined" +#endif + +#if defined (__PX4_LINUX) || defined(__PX4_NUTTX) +#include +#else +#define prctl(_action, _string, _pid) #endif typedef int px4_task_t; @@ -93,11 +105,11 @@ __EXPORT void px4_systemreset(bool to_bootloader) noreturn_function; /** Starts a task and performs any specific accounting, scheduler setup, etc. */ __EXPORT px4_task_t px4_task_spawn_cmd(const char *name, - int priority, - int scheduler, - int stack_size, - px4_main_t entry, - char * const argv[]); + int priority, + int scheduler, + int stack_size, + px4_main_t entry, + char *const argv[]); /** Deletes a task - does not do resource cleanup **/ __EXPORT int px4_task_delete(px4_task_t pid); @@ -111,5 +123,8 @@ __EXPORT void px4_task_exit(int ret); /** Show a list of running tasks **/ __EXPORT void px4_show_tasks(void); +/** See if a task is running **/ +__EXPORT bool px4_task_is_running(const char *taskname); + __END_DECLS diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index 0c9b7f24c943..d57f8e91034f 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -8,18 +8,29 @@ #define px4_clock_gettime clock_gettime #define px4_clock_settime clock_settime +#elif defined(__PX4_DARWIN) + +__BEGIN_DECLS + +#define clockid_t unsigned +#define CLOCK_REALTIME 0 + +int px4_clock_gettime(clockid_t clk_id, struct timespec *tp); +int px4_clock_settime(clockid_t clk_id, struct timespec *tp); + +__EXPORT unsigned int sleep(unsigned int sec); + +__END_DECLS + #elif defined(__PX4_QURT) #include -#define CLOCK_REALTIME 1 - __BEGIN_DECLS #if 0 #if !defined(__cplusplus) -struct timespec -{ +struct timespec { time_t tv_sec; long tv_nsec; }; @@ -28,7 +39,6 @@ struct timespec int px4_clock_gettime(clockid_t clk_id, struct timespec *tp); int px4_clock_settime(clockid_t clk_id, struct timespec *tp); -__EXPORT int usleep(useconds_t usec); __EXPORT unsigned int sleep(unsigned int sec); __END_DECLS diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index b9ae6b4b6cb0..3e3170f2de03 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -41,10 +41,15 @@ #include #include #include -#elif defined(__PX4_POSIX) || defined(__PX4_QURT) +#elif defined(__PX4_POSIX) #include #include +#include + +#ifdef __PX4_QURT +#include +#endif __BEGIN_DECLS @@ -52,10 +57,9 @@ __BEGIN_DECLS #define LPWORK 1 #define NWORKERS 2 -struct wqueue_s -{ - pid_t pid; /* The task ID of the worker thread */ - struct dq_queue_s q; /* The queue of pending work */ +struct wqueue_s { + pid_t pid; /* The task ID of the worker thread */ + struct dq_queue_s q; /* The queue of pending work */ }; extern struct wqueue_s g_work[NWORKERS]; @@ -64,13 +68,12 @@ extern struct wqueue_s g_work[NWORKERS]; typedef void (*worker_t)(void *arg); -struct work_s -{ - struct dq_entry_s dq; /* Implements a doubly linked list */ - worker_t worker; /* Work callback */ - void *arg; /* Callback argument */ - uint32_t qtime; /* Time work queued */ - uint32_t delay; /* Delay until work performed */ +struct work_s { + struct dq_entry_s dq; /* Implements a doubly linked list */ + worker_t worker; /* Work callback */ + void *arg; /* Callback argument */ + uint64_t qtime; /* Time work queued */ + uint32_t delay; /* Delay until work performed */ }; /**************************************************************************** @@ -139,6 +142,6 @@ int work_lpthread(int argc, char *argv[]); __END_DECLS -#else +#else #error "Unknown target OS" #endif diff --git a/src/platforms/qurt/dspal/dspal_stub.c b/src/platforms/qurt/dspal/dspal_stub.c new file mode 100644 index 000000000000..13542cdfd3af --- /dev/null +++ b/src/platforms/qurt/dspal/dspal_stub.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#include +#include + +//#define STACK_SIZE 0x8000 +//static char __attribute__ ((aligned (16))) stack1[STACK_SIZE]; + +static void do_dlopen() +{ +#if 0 + void *handle; + char *error; + void (*entry_function)() = NULL; + + handle = dlopen("libdspal_client.so", RTLD_LAZY); + + if (!handle) { + printf("Error opening libdspal_client.so\n"); + return 1; + } + + entry_function = dlsym(handle, "dspal_entry"); + + if (((error = dlerror()) != NULL) || (entry_function == NULL)) { + printf("Error dlsym for dspal_entry"); + ret = 2; + } + + dlclose(handle); +#endif +} + + +int main(int argc, char *argv[]) +{ + int ret = 0; + char *builtin[] = {"libgcc.so", "libc.so", "libstdc++.so"}; + + printf("In DSPAL main\n"); + dlinit(3, builtin); + + do_dlopen(); + return ret; +} + diff --git a/src/platforms/qurt/include/crc32.h b/src/platforms/qurt/include/crc32.h index bf828e3e0e9f..34e080b1c2e3 100644 --- a/src/platforms/qurt/include/crc32.h +++ b/src/platforms/qurt/include/crc32.h @@ -63,7 +63,7 @@ extern "C" { ****************************************************************************/ EXTERN uint32_t crc32part(const uint8_t *src, size_t len, - uint32_t crc32val); + uint32_t crc32val); /**************************************************************************** * Name: crc32 diff --git a/src/platforms/qurt/include/hrt_work.h b/src/platforms/qurt/include/hrt_work.h new file mode 100644 index 000000000000..019056501aa7 --- /dev/null +++ b/src/platforms/qurt/include/hrt_work.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ + +#include +#include +#include + +#pragma once + +__BEGIN_DECLS + +extern sem_t _hrt_work_lock; +extern struct wqueue_s g_hrt_work; + +void hrt_work_queue_init(void); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); +void hrt_work_cancel(struct work_s *work); + +static inline void hrt_work_lock(void); +static inline void hrt_work_unlock(void); + +static inline void hrt_work_lock() +{ + //PX4_INFO("hrt_work_lock"); + sem_wait(&_hrt_work_lock); +} + +static inline void hrt_work_unlock() +{ + //PX4_INFO("hrt_work_unlock"); + sem_post(&_hrt_work_lock); +} + +__END_DECLS + diff --git a/src/platforms/qurt/include/i2c.h b/src/platforms/qurt/include/i2c.h index ee3e4196dd61..5021cf18146e 100644 --- a/src/platforms/qurt/include/i2c.h +++ b/src/platforms/qurt/include/i2c.h @@ -34,10 +34,9 @@ ****************************************************************************/ #pragma once -struct i2c_msg_s -{ - uint16_t addr; /* Slave address */ - uint16_t flags; /* See I2C_M_* definitions */ - uint8_t *buffer; - int length; +struct i2c_msg_s { + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; }; diff --git a/src/platforms/qurt/include/queue.h b/src/platforms/qurt/include/queue.h index 4d95541e0203..30dc264f3f37 100644 --- a/src/platforms/qurt/include/queue.h +++ b/src/platforms/qurt/include/queue.h @@ -69,30 +69,26 @@ * Global Type Declarations ************************************************************************/ -struct sq_entry_s -{ - FAR struct sq_entry_s *flink; +struct sq_entry_s { + FAR struct sq_entry_s *flink; }; typedef struct sq_entry_s sq_entry_t; -struct dq_entry_s -{ - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; }; typedef struct dq_entry_s dq_entry_t; -struct sq_queue_s -{ - FAR sq_entry_t *head; - FAR sq_entry_t *tail; +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; }; typedef struct sq_queue_s sq_queue_t; -struct dq_queue_s -{ - FAR dq_entry_t *head; - FAR dq_entry_t *tail; +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; }; typedef struct dq_queue_s dq_queue_t; @@ -112,11 +108,11 @@ EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); + sq_queue_t *queue); EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); diff --git a/src/platforms/qurt/include/qurt_log.h b/src/platforms/qurt/include/qurt_log.h new file mode 100644 index 000000000000..29ef6302e145 --- /dev/null +++ b/src/platforms/qurt/include/qurt_log.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#ifndef QURT_LOG_H +#define QURT_LOG_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//void qurt_log(int level, const char *file, int line, const char *format, ...); + +// declaration to make the compiler happy. This symbol is part of the adsp static image. +void HAP_debug(const char *msg, int level, const char *filename, int line); + +static __inline void qurt_log(int level, const char *file, int line, + const char *format, ...) +{ + char buf[256]; + va_list args; + va_start(args, format); + vsnprintf(buf, sizeof(buf), format, args); + va_end(args); + HAP_debug(buf, level, file, line); +} + +#ifdef __cplusplus +} +#endif + +#endif // QURT_LOG_H + diff --git a/src/platforms/qurt/include/sched.h b/src/platforms/qurt/include/sched.h index 7d7724c5fae0..b67db23b1dc6 100644 --- a/src/platforms/qurt/include/sched.h +++ b/src/platforms/qurt/include/sched.h @@ -3,8 +3,7 @@ #define SCHED_FIFO 1 #define SCHED_RR 2 -struct sched_param -{ +struct sched_param { int sched_priority; }; diff --git a/src/platforms/qurt/px4_layer/CMakeLists.txt b/src/platforms/qurt/px4_layer/CMakeLists.txt new file mode 100644 index 000000000000..b1b99b63f2b5 --- /dev/null +++ b/src/platforms/qurt/px4_layer/CMakeLists.txt @@ -0,0 +1,73 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +set(QURT_LAYER_SRCS + px4_qurt_impl.cpp + px4_qurt_tasks.cpp + lib_crc32.c + drv_hrt.c + qurt_stubs.c + main.cpp + params.c + ) +if ("${QURT_ENABLE_STUBS}" STREQUAL "1") + list(APPEND QURT_LAYER_SRCS + ../stubs/stubs_posix.c + ../stubs/stubs_qurt.c + ) +endif() + +# For Eagle, the commands are specific to the build config label +# e.g. config_qurt_eagle_hil uses commands_hil.c +if ("${BOARD}" STREQUAL "eagle") + + # The CI test target can use the hil commands + if ("${LABEL}" STREQUAL "travis") + set(CONFIG_SRC commands_hil.c) + else() + set(CONFIG_SRC commands_${LABEL}.c) + endif() + +endif() + +px4_add_module( + MODULE platforms__qurt__px4_layer + COMPILE_FLAGS + -Os + SRCS + ${QURT_LAYER_SRCS} + ${CONFIG_SRC} + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/qurt/px4_layer/commands_hil.c b/src/platforms/qurt/px4_layer/commands_hil.c new file mode 100644 index 000000000000..87aa6479f881 --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_hil.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 commands_muorb_test.c + * Commands to run for the "qurt_muorb_test" config + * + * @author Mark Charlebois + */ + +const char *get_commands(void); +const char *get_commands() +{ + + static const char *commands = + "uorb start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" +// "rgbled start\n" +// "tone_alarm start\n" + "commander start -hil\n" + "sensors start\n" + //"ekf_att_pos_estimator start\n" + "attitude_estimator_q start\n" + "position_estimator_inav start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "sleep 1\n" + "pwm_out_sim mode_pwm\n" + "param set RC1_MAX 2015\n" + "param set RC1_MIN 996\n" + "param set RC1_TRIM 1502\n" + "param set RC1_REV -1\n" + "param set RC2_MAX 2016 \n" + "param set RC2_MIN 995\n" + "param set RC2_TRIM 1500\n" + "param set RC3_MAX 2003\n" + "param set RC3_MIN 992\n" + "param set RC3_TRIM 992\n" + "param set RC4_MAX 2011\n" + "param set RC4_MIN 997\n" + "param set RC4_TRIM 1504\n" + "param set RC4_REV -1\n" + "param set RC6_MAX 2016\n" + "param set RC6_MIN 992\n" + "param set RC6_TRIM 1504\n" + "param set RC_CHAN_CNT 8\n" + "param set RC_MAP_MODE_SW 5\n" + "param set RC_MAP_POSCTL_SW 7\n" + "param set RC_MAP_RETURN_SW 8\n" + "param set MC_YAW_P 1.5\n" + "param set MC_PITCH_P 3.0\n" + "param set MC_ROLL_P 3.0\n" + "param set MC_YAWRATE_P 0.2\n" + "param set MC_PITCHRATE_P 0.03\n" + "param set MC_ROLLRATE_P 0.03\n" + "param set ATT_W_ACC 0.0002\n" + "param set ATT_W_MAG 0.002\n" + "param set ATT_W_GYRO_BIAS 0.05\n" + "sleep 1\n" + + + "param set MAV_TYPE 2\n" + "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n" + "list_devices\n" + "list_files\n" + "list_tasks\n" + "list_topics\n" + "sleep 10\n" + "list_tasks\n" + "sleep 10\n" + + ; + + return commands; + +} diff --git a/src/platforms/qurt/px4_layer/commands_muorb.c b/src/platforms/qurt/px4_layer/commands_muorb.c new file mode 100644 index 000000000000..de6381a35a1b --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_muorb.c @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 commands_muorb_test.c + * Commands to run for the "qurt_muorb_test" config + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + static const char *commands = + "uorb start\n" + "muorb_test start\n"; + + /* + "hil mode_pwm\n" + "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n"; + */ + /* + "param show\n" + "param set CAL_GYRO_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "gyrosim start\n" + "accelsim start\n" + "rgbled start\n" + "tone_alarm start\n" + "simulator start -s\n" + "commander start\n" + "sensors start\n" + "ekf_att_pos_estimator start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "param set MAV_TYPE 2\n" + "param set RC1_MAX 2015\n" + "param set RC1_MIN 996\n" + "param set RC_TRIM 1502\n" + */ + + return commands; + /*====================================== Working set + ======================================*/ + + //"muorb_test start\n" + //"gyrosim start\n" + //"adcsim start\n" + +} diff --git a/src/platforms/qurt/px4_layer/commands_release.c b/src/platforms/qurt/px4_layer/commands_release.c new file mode 100644 index 000000000000..0e6ee8402c14 --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_release.c @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 commands_muorb_test.c + * Commands to run for the "qurt_muorb_test" config + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + + static const char *commands = + "uorb start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "commander start\n" + "param set RC_RECEIVER_TYPE 1\n" + "rc_receiver start -D /dev/tty-1\n" + "attitude_estimator_q start\n" + "position_estimator_inav start\n" + //"ekf_att_pos_estimator start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "sleep 1\n" + + // Channel mapping for Spektrum DX8 + "param set RC_MAP_THROTTLE 1\n" + "param set RC_MAP_ROLL 2\n" + "param set RC_MAP_PITCH 3\n" + "param set RC_MAP_YAW 4\n" + "param set RC_MAP_MODE_SW 5\n" + "param set RC_MAP_POSCTL_SW 6\n" + + // RC calibration for Spektrum DX8 + "param set RC1_MAX 852\n" + "param set RC1_MIN 171\n" + "param set RC1_TRIM 171\n" + "param set RC1_REV 1\n" + "param set RC2_MAX 852\n" + "param set RC2_MIN 171\n" + "param set RC2_TRIM 512\n" + "param set RC2_REV -1\n" + "param set RC3_MAX 852\n" + "param set RC3_MIN 171\n" + "param set RC3_TRIM 512\n" + "param set RC3_REV 1\n" + "param set RC4_MAX 852\n" + "param set RC4_MIN 171\n" + "param set RC4_TRIM 514\n" + "param set RC4_REV -1\n" + "param set RC5_MAX 852\n" + "param set RC5_MIN 171\n" + "param set RC5_TRIM 512\n" + "param set RC5_REV 1\n" + "param set RC6_MAX 852\n" + "param set RC6_MIN 171\n" + "param set RC6_TRIM 171\n" + "param set RC6_REV 1\n" + + // // RC calibration for DX6i + // "param set RC1_MAX 2015\n" + // "param set RC1_MIN 996\n" + // "param set RC1_TRIM 1502\n" + // "param set RC1_REV -1\n" + // "param set RC2_MAX 2016 \n" + // "param set RC2_MIN 995\n" + // "param set RC2_TRIM 1500\n" + // "param set RC3_MAX 2003\n" + // "param set RC3_MIN 992\n" + // "param set RC3_TRIM 992\n" + // "param set RC4_MAX 2011\n" + // "param set RC4_MIN 997\n" + // "param set RC4_TRIM 1504\n" + // "param set RC4_REV -1\n" + // "param set RC6_MAX 2016\n" + // "param set RC6_MIN 992\n" + // "param set RC6_TRIM 1504\n" + // "param set RC_CHAN_CNT 8\n" + // "param set RC_MAP_MODE_SW 5\n" + // "param set RC_MAP_POSCTL_SW 7\n" + // "param set RC_MAP_RETURN_SW 8\n" + + "sensors start\n" + "param set MC_YAW_P 7.0\n" + "param set MC_YAWRATE_P 0.1125\n" + "param set MC_YAWRATE_I 0.0\n" + "param set MC_YAWRATE_D 0\n" + "param set MC_PITCH_P 6.0\n" + "param set MC_PITCHRATE_P 0.125\n" + "param set MC_PITCHRATE_I 0.0\n" + "param set MC_PITCHRATE_D 0.0\n" + "param set MC_ROLL_P 6.0\n" + "param set MC_ROLLRATE_P 0.1125\n" + "param set MC_ROLLRATE_I 0.0\n" + "param set MC_ROLLRATE_D 0.0\n" + "param set ATT_W_MAG 0.00\n" + "param set PE_MAG_NOISE 1.0f\n" // ekf_att_pos only + "param set SENS_BOARD_ROT 6\n" + + "param set CAL_GYRO0_XOFF 0.0\n" + "param set CAL_GYRO0_YOFF 0.0\n" + "param set CAL_GYRO0_ZOFF 0.0\n" + "param set CAL_GYRO0_XSCALE 1.000000\n" + "param set CAL_GYRO0_YSCALE 1.000000\n" + "param set CAL_GYRO0_ZSCALE 1.000000\n" + "param set CAL_ACC0_XOFF 0.0\n" + "param set CAL_ACC0_YOFF 0.0\n" + "param set CAL_ACC0_ZOFF 0.0\n" + "param set CAL_ACC0_XSCALE 1.0\n" + "param set CAL_ACC0_YSCALE 1.0\n" + "param set CAL_ACC0_ZSCALE 1.0\n" + // "param set CAL_ACC0_XOFF 0.064189\n" + // "param set CAL_ACC0_YOFF 0.153990\n" + // "param set CAL_ACC0_ZOFF -0.000567\n" + "param set MPU_GYRO_LPF_ENUM 4\n" + "param set MPU_ACC_LPF_ENUM 4\n" + "param set MPU_SAMPLE_RATE_ENUM 2\n" + "sleep 1\n" + "mpu9x50 start -D /dev/spi-1\n" + "uart_esc start -D /dev/tty-2\n" + "csr_gps start -D /dev/tty-3\n" + "param set MAV_TYPE 2\n" + "list_devices\n" + "list_files\n" + "list_tasks\n" + "list_topics\n" + + ; + + return commands; + +} + +/* +simulator numbers + "param set MC_YAW_P 1.5\n" + "param set MC_PITCH_P 3.0\n" + "param set MC_ROLL_P 3.0\n" + "param set MC_YAWRATE_P 0.2\n" + "param set MC_PITCHRATE_P 0.03\n" + "param set MC_ROLLRATE_P 0.03\n" +*/ diff --git a/src/platforms/qurt/px4_layer/commands_test.c b/src/platforms/qurt/px4_layer/commands_test.c new file mode 100644 index 000000000000..1c4c50f02265 --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_test.c @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 commands_default.c + * Commands to run for the "qurt_default" config + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + static const char *commands = + "hello start\n" + "uorb start\n" + "simulator start -s\n" + "barosim start\n" + "adcsim start\n" + "accelsim start\n" + "gyrosim start\n" + "list_devices\n" + "list_topics\n" + "list_tasks\n" + "param show *\n" + "rgbled start\n" +#if 0 + "sensors start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "hil mode_pwm" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "mavlink start -d /tmp/ttyS0\n" + "commander start\n" +#endif + ; + + return commands; +} diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index 3edf34f64c3a..c15c81817217 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -37,10 +37,12 @@ * High-resolution timer with callouts and timekeeping. */ +#include #include #include #include #include +#include #include #include @@ -82,9 +84,7 @@ static void hrt_unlock(void) hrt_abstime hrt_absolute_time(void) { struct timespec ts; - - // FIXME - clock_gettime unsupported in QuRT - //clock_gettime(CLOCK_MONOTONIC, &ts); + clock_gettime(CLOCK_MONOTONIC, &ts); return ts_to_abstime(&ts); } @@ -219,7 +219,7 @@ hrt_call_enter(struct hrt_call *entry) * * This routine simulates a timer interrupt handler */ -static void +static void hrt_tim_isr(void *p) { @@ -246,10 +246,10 @@ hrt_call_reschedule() hrt_abstime now = hrt_absolute_time(); struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); hrt_abstime deadline = now + HRT_INTERVAL_MAX; - uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); + uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX); //printf("hrt_call_reschedule\n"); - + /* * Determine what the next deadline will be. * @@ -267,18 +267,18 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); + ticks = USEC2TICK(HRT_INTERVAL_MIN); } else if (next->deadline < deadline) { //lldbg("due soon\n"); - ticks = USEC2TICK((next->deadline - now)*1000); + ticks = USEC2TICK((next->deadline - now)); } } - // There is no timer ISR, so simulate one by putting an event on the + // There is no timer ISR, so simulate one by putting an event on the // high priority work queue //printf("ticks = %u\n", ticks); - work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); + work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); } static void @@ -286,6 +286,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte { //printf("hrt_call_internal\n"); hrt_lock(); + //printf("hrt_call_internal after lock\n"); /* if the entry is currently queued, remove it */ /* note that we are using a potentially uninitialised @@ -295,8 +296,9 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte queue for the uninitialised entry->link but we don't do anything actually unsafe. */ - if (entry->deadline != 0) + if (entry->deadline != 0) { sq_rem(&entry->link, &callout_queue); + } entry->deadline = deadline; entry->period = interval; @@ -360,17 +362,20 @@ hrt_call_invoke(void) hrt_abstime deadline; hrt_lock(); + while (true) { /* get the current time */ hrt_abstime now = hrt_absolute_time(); call = (struct hrt_call *)sq_peek(&callout_queue); - if (call == NULL) + if (call == NULL) { break; + } - if (call->deadline > now) + if (call->deadline > now) { break; + } sq_rem(&call->link, &callout_queue); //lldbg("call pop\n"); @@ -404,6 +409,7 @@ hrt_call_invoke(void) hrt_call_enter(call); } } + hrt_unlock(); } diff --git a/src/platforms/qurt/px4_layer/lib_crc32.c b/src/platforms/qurt/px4_layer/lib_crc32.c index 4ba6fbf6df79..c14ebfceeba2 100644 --- a/src/platforms/qurt/px4_layer/lib_crc32.c +++ b/src/platforms/qurt/px4_layer/lib_crc32.c @@ -48,46 +48,45 @@ #include // Needed for Linux -#define FAR +#define FAR /************************************************************************************************ * Private Data ************************************************************************************************/ - -static const uint32_t crc32_tab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d + +static const uint32_t crc32_tab[] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d }; /************************************************************************************************ @@ -103,13 +102,13 @@ static const uint32_t crc32_tab[] = uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) { - size_t i; + size_t i; + + for (i = 0; i < len; i++) { + crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); + } - for (i = 0; i < len; i++) - { - crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); - } - return crc32val; + return crc32val; } /************************************************************************************************ @@ -122,5 +121,5 @@ uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) uint32_t crc32(FAR const uint8_t *src, size_t len) { - return crc32part(src, len, 0); + return crc32part(src, len, 0); } diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 538b64229f33..fc8801b60e54 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -1,6 +1,5 @@ /**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Copyright (C) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,46 +31,58 @@ ****************************************************************************/ /** * @file main.cpp - * Basic shell to execute builtin "apps" + * Basic shell to execute builtin "apps" * * @author Mark Charlebois */ #include #include +#include #include +#include +#include #include #include #include #include +#include using namespace std; -extern void init_app_map(map &apps); -extern void list_builtins(map &apps); +extern void init_app_map(map &apps); +extern void list_builtins(map &apps); +static px4_task_t g_dspal_task = -1; __BEGIN_DECLS // The commands to run are specified in a target file: commands_.c extern const char *get_commands(void); +// Enable external library hook +void qurt_external_hook(void) __attribute__((weak)); __END_DECLS -static void run_cmd(map &apps, const vector &appargs) { +static void run_cmd(map &apps, const vector &appargs) +{ // command is appargs[0] string command = appargs[0]; + if (apps.find(command) != apps.end()) { - const char *arg[2+1]; + const char *arg[2 + 1]; unsigned int i = 0; + while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); - //printf(" arg = '%s'\n", arg[i]); + PX4_WARN(" arg = '%s'\n", arg[i]); ++i; } + arg[i] = (char *)0; - apps[command](i,(char **)arg); - } - else - { + //PX4_DEBUG_PRINTF(i); + apps[command](i, (char **)arg); + + } else { + PX4_WARN("NOT FOUND."); list_builtins(apps); } } @@ -80,22 +91,23 @@ void eat_whitespace(const char *&b, int &i) { // Eat whitespace while (b[i] == ' ' || b[i] == '\t') { ++i; } + b = &b[i]; - i=0; + i = 0; } -static void process_commands(map &apps, const char *cmds) +static void process_commands(map &apps, const char *cmds) { vector appargs; - int i=0; + int i = 0; const char *b = cmds; bool found_first_char = false; - char arg[20]; + char arg[256]; // Eat leading whitespace eat_whitespace(b, i); - for(;;) { + for (;;) { // End of command line if (b[i] == '\n' || b[i] == '\0') { strncpy(arg, b, i); @@ -104,17 +116,26 @@ static void process_commands(map &apps, const char *cmds) // If we have a command to run if (appargs.size() > 0) { + PX4_WARN("Processing command: %s", appargs[0].c_str()); + + for (int ai = 1; ai < (int)appargs.size(); ai++) { + PX4_WARN(" > arg: %s", appargs[ai].c_str()); + } + run_cmd(apps, appargs); } + appargs.clear(); + if (b[i] == '\n') { eat_whitespace(b, ++i); continue; - } - else { + + } else { break; } } + // End of arg else if (b[i] == ' ') { strncpy(arg, b, i); @@ -123,22 +144,76 @@ static void process_commands(map &apps, const char *cmds) eat_whitespace(b, ++i); continue; } + ++i; } } -namespace px4 { +namespace px4 +{ extern void init_once(void); }; -int main(int argc, char **argv) +__BEGIN_DECLS +int dspal_main(int argc, char *argv[]); +__END_DECLS + + +int dspal_entry(int argc, char *argv[]) { - printf("In main\n"); - map apps; + PX4_INFO("In main\n"); + map apps; init_app_map(apps); px4::init_once(); - px4::init(argc, argv, "mainapp"); + px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); - for (;;) {} + sleep(1); // give time for all commands to execute before starting external function + + if (qurt_external_hook) { + qurt_external_hook(); + } + + for (;;) { + sleep(1); + } + + return 0; } +static void usage() +{ + PX4_WARN("Usage: dspal {start |stop}"); +} + + +extern "C" { + + int dspal_main(int argc, char *argv[]) + { + int ret = 0; + + if (argc == 2 && strcmp(argv[1], "start") == 0) { + g_dspal_task = px4_task_spawn_cmd("dspal", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + dspal_entry, + argv); + + } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { + if (g_dspal_task < 0) { + PX4_WARN("start up thread not running"); + + } else { + px4_task_delete(g_dspal_task); + g_dspal_task = -1; + } + + } else { + usage(); + ret = -1; + } + + return ret; + } +} diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 714263edcbde..47902057cc00 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -35,29 +35,32 @@ # NuttX / uORB adapter library # +MODULE_NAME = dspal + SRCDIR=$(dir $(MODULE_MK)) SRCS = \ px4_qurt_impl.cpp \ px4_qurt_tasks.cpp \ - work_thread.c \ - work_queue.c \ - work_cancel.c \ lib_crc32.c \ drv_hrt.c \ - queue.c \ - dq_addlast.c \ - dq_remfirst.c \ - sq_addlast.c \ - sq_remfirst.c \ - sq_addafter.c \ - dq_rem.c \ - main.cpp + qurt_stubs.c \ + main.cpp ifeq ($(CONFIG),qurt_hello) SRCS += commands_hello.c endif ifeq ($(CONFIG),qurt_default) SRCS += commands_default.c endif +ifeq ($(CONFIG),qurt_muorb_test) +SRCS += commands_muorb_test.c +endif +ifeq ($(CONFIG),qurt_hil) +SRCS += commands_hil.c +endif +ifeq ($(CONFIG),qurt_adsp) +SRCS += commands_adsp.c +endif + MAXOPTIMIZATION = -Os diff --git a/src/platforms/qurt/px4_layer/params.c b/src/platforms/qurt/px4_layer/params.c new file mode 100644 index 000000000000..654b07df00c8 --- /dev/null +++ b/src/platforms/qurt/px4_layer/params.c @@ -0,0 +1,43 @@ +/**************************************************************************** + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ + +#include + +// This is added because it is a parameter used by commander, yet created by mavlink. Since mavlink is not +// running on QURT, we need to manually define it so it is available to commander. "2" is for quadrotor. + +// Following is hack to prevent duplicate parameter definition error in param parser +/** + * @board QuRT_App + */ +PARAM_DEFINE_INT32(MAV_TYPE, 2); + diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 3ff09b3d4f78..be419d406926 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -48,23 +48,26 @@ #include #include #include -#include #include "systemlib/param/param.h" +#include "hrt_work.h" +#include "px4_log.h" + + +//extern pthread_t _shell_task_id; + __BEGIN_DECLS +extern uint64_t get_ticks_per_us(); -// FIXME - sysconf(_SC_CLK_TCK) not supported -long PX4_TICKS_PER_SEC = 1000; +//long PX4_TICKS_PER_SEC = 1000L; -void usleep(useconds_t usec) { - qurt_timer_sleep(usec); -} +unsigned int sleep(unsigned int sec) +{ + for (unsigned int i = 0; i < sec; i++) { + usleep(1000000); + } -unsigned int sleep(unsigned int sec) -{ - for (unsigned int i=0; i< sec; i++) - qurt_timer_sleep(1000000); - return 0; + return 0; } extern void hrt_init(void); @@ -79,7 +82,7 @@ void qurt_log(const char *fmt, ...) } #endif -extern int _posix_init(void); +//extern int _posix_init(void); __END_DECLS @@ -94,10 +97,17 @@ void init_once(void); void init_once(void) { // Required for QuRT - _posix_init(); + //_posix_init(); + PX4_WARN("Before calling work_queue_init"); + +// _shell_task_id = pthread_self(); +// PX4_INFO("Shell id is %lu", _shell_task_id); work_queues_init(); + PX4_WARN("Before calling hrt_init"); + hrt_work_queue_init(); hrt_init(); + PX4_WARN("after calling hrt_init"); } void init(int argc, char *argv[], const char *app_name) @@ -134,9 +144,11 @@ dm_write( size_t strnlen(const char *s, size_t maxlen) { - size_t i=0; - while (s[i] != '\0' && i < maxlen) + size_t i = 0; + + while (s[i] != '\0' && i < maxlen) { ++i; + } return i; } @@ -146,7 +158,7 @@ int ioctl(int a, int b, unsigned long c) return -1; } -int write(int a, char const* b, int c) +int write(int a, char const *b, int c) { return -1; } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index d688f1104a41..4216bbf599fe 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -43,7 +43,11 @@ #include #include #include + +#if !defined(__PX4_QURT) #include +#endif + #include #include #include @@ -57,9 +61,12 @@ #define MAX_CMD_LEN 100 -#define PX4_MAX_TASKS 100 -struct task_entry -{ +#define PX4_MAX_TASKS 50 +#define SHELL_TASK_ID (PX4_MAX_TASKS+1) + +pthread_t _shell_task_id = 0; + +struct task_entry { pthread_t pid; std::string name; bool isused; @@ -68,25 +75,30 @@ struct task_entry static task_entry taskmap[PX4_MAX_TASKS]; -typedef struct -{ +typedef struct { px4_main_t entry; int argc; char *argv[]; - // strings are allocated after the + // strings are allocated after the } pthdata_t; -static void *entry_adapter ( void *ptr ) +static void *entry_adapter(void *ptr) { - pthdata_t *data; - data = (pthdata_t *) ptr; + pthdata_t *data; + data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); + PX4_WARN("Before waiting infinte busy loop"); + //for( ;; ) + //{ + // volatile int x = 0; + // ++x; + // } free(ptr); - px4_task_exit(0); + px4_task_exit(0); return NULL; -} +} void px4_systemreset(bool to_bootloader) @@ -94,7 +106,8 @@ px4_systemreset(bool to_bootloader) PX4_WARN("Called px4_system_reset"); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, + char *const argv[]) { int rv; int argc = 0; @@ -102,73 +115,96 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p = (char *)argv; + char *p = (char *)argv; PX4_DEBUG("Creating %s\n", name); - pthread_t task; + pthread_t task; pthread_attr_t attr; struct sched_param param; // Calculate argc while (p != (char *)0) { p = argv[argc]; - if (p == (char *)0) + + if (p == (char *)0) { break; + } + ++argc; - len += strlen(p)+1; + len += strlen(p) + 1; } - structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); + + structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); pthdata_t *taskdata; - + // not safe to pass stack data to the thread creation - taskdata = (pthdata_t *)malloc(structsize+len); - offset = ((unsigned long)taskdata)+structsize; + taskdata = (pthdata_t *)malloc(structsize + len); + offset = ((unsigned long)taskdata) + structsize; - taskdata->entry = entry; + taskdata->entry = entry; taskdata->argc = argc; - for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); - offset+=strlen(argv[i])+1; + offset += strlen(argv[i]) + 1; } + // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; rv = pthread_attr_init(&attr); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); return (rv < 0) ? rv : -rv; } + #if 0 rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); return (rv < 0) ? rv : -rv; } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); return (rv < 0) ? rv : -rv; } + #endif + size_t fixed_stacksize = -1; + pthread_attr_getstacksize(&attr, &fixed_stacksize); + PX4_WARN("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size); + fixed_stacksize = 8 * 1024; + fixed_stacksize = (fixed_stacksize < (size_t)stack_size) ? (size_t)stack_size : fixed_stacksize; + + PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize); + pthread_attr_setstacksize(&attr, fixed_stacksize); + //pthread_attr_setstacksize(&attr, stack_size); + param.sched_priority = priority; rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); return (rv < 0) ? rv : -rv; } - rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); + rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); + if (rv != 0) { return (rv < 0) ? rv : -rv; } - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { return -ENOSPC; } - return i; + + return i; } int px4_task_delete(px4_task_t id) @@ -188,15 +226,18 @@ int px4_task_delete(px4_task_t id) pthread_t pid; PX4_WARN("Called px4_task_delete"); - if (id < PX4_MAX_TASKS && taskmap[id].isused) + if (id < PX4_MAX_TASKS && taskmap[id].isused) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { + if (pthread_self() == pid) { taskmap[id].isused = false; pthread_exit(0); + } else { rv = pthread_cancel(pid); } @@ -208,20 +249,21 @@ int px4_task_delete(px4_task_t id) void px4_task_exit(int ret) { - int i; + int i; pthread_t pid = pthread_self(); // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { PX4_ERR("px4_task_exit: self task not found!"); - } - else { + + } else { PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); } @@ -232,12 +274,14 @@ int px4_task_kill(px4_task_t id, int sig) { int rv = 0; pthread_t pid; - PX4_DEBUG("Called px4_task_kill %d", sig); + PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str()); - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) + if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel rv = pthread_kill(pid, sig); @@ -251,14 +295,53 @@ void px4_show_tasks() int count = 0; PX4_INFO("Active Tasks:"); - for (idx=0; idx < PX4_MAX_TASKS; idx++) - { + + for (idx = 0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - PX4_INFO(" %-10s %lu", taskmap[idx].name.c_str(), taskmap[idx].pid); + PX4_INFO(" %-10s %d", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } - if (count == 0) + + if (count == 0) { PX4_INFO(" No running tasks"); + } + +} + +__BEGIN_DECLS + +int px4_getpid() +{ + pthread_t pid = pthread_self(); +// +// if (pid == _shell_task_id) +// return SHELL_TASK_ID; + + // Get pthread ID from the opaque ID + for (int i = 0; i < PX4_MAX_TASKS; ++i) { + if (taskmap[i].isused && taskmap[i].pid == pid) { + return i; + } + } + PX4_ERR("px4_getpid() called from unknown thread context!"); + return -EINVAL; } + + +const char *getprogname(); +const char *getprogname() +{ + pthread_t pid = pthread_self(); + + for (int i = 0; i < PX4_MAX_TASKS; i++) { + if (taskmap[i].isused && taskmap[i].pid == pid) { + return taskmap[i].name.c_str(); + } + } + + return "Unknown App"; +} +__END_DECLS + diff --git a/src/platforms/qurt/px4_layer/qurt_stubs.c b/src/platforms/qurt/px4_layer/qurt_stubs.c new file mode 100644 index 000000000000..ab449f67a026 --- /dev/null +++ b/src/platforms/qurt/px4_layer/qurt_stubs.c @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#include "px4_log.h" +#include + +void block_indefinite(void); +void _Read_uleb(void); +void _Parse_fde_instr(void); +void _Parse_csd(void); +void _Valbytes(void); +void _Get_eh_data(void); +void _Parse_lsda(void); +void __cxa_guard_release(void); +void _Read_enc_ptr(void); +void _Read_sleb(void); +void __cxa_guard_acquire(void); +void __cxa_pure_virtual(void); + +void block_indefinite(void) +{ + sem_t forever; + sem_init(&forever, 0, 0); + sem_wait(&forever); +} + +void _Read_uleb(void) +{ + PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); + block_indefinite(); +} + +void _Parse_fde_instr(void) +{ + PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); + block_indefinite(); +} + +void _Parse_csd(void) +{ + PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); + block_indefinite(); +} + +void _Valbytes(void) +{ + PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); + block_indefinite(); +} + +void _Get_eh_data(void) +{ + PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); + block_indefinite(); +} + +void _Parse_lsda(void) +{ + PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); + block_indefinite(); +} + +void __cxa_guard_release(void) +{ + PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); + block_indefinite(); +} + +void _Read_enc_ptr(void) +{ + PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); + block_indefinite(); +} + +void _Read_sleb(void) +{ + PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); + block_indefinite(); +} + +void __cxa_guard_acquire(void) +{ + PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); + block_indefinite(); +} + +void __cxa_pure_virtual() +{ + PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); + block_indefinite(); +} diff --git a/src/platforms/qurt/stubs/stubs_posix.c b/src/platforms/qurt/stubs/stubs_posix.c new file mode 100644 index 000000000000..0b9faab882a2 --- /dev/null +++ b/src/platforms/qurt/stubs/stubs_posix.c @@ -0,0 +1,99 @@ +#include +#include +#include +#include +#include + +int sem_init(sem_t *sem, int pshared, unsigned int value) +{ + return 1; +} + +int sem_wait(sem_t *sem) +{ + return 1; +} + +int sem_destroy(sem_t *sem) +{ + return 1; +} + +int sem_post(sem_t *sem) +{ + return 1; +} + +int sem_getvalue(sem_t *sem, int *sval) +{ + return 1; +} + +int usleep(useconds_t usec) +{ + return 0; +} + +pthread_t pthread_self(void) +{ + pthread_t x = 0; + return x; +} + + +int pthread_kill(pthread_t thread, int sig) +{ + return 1; +} + +void pthread_exit(void *retval) +{ +} + +int pthread_join(pthread_t thread, void **retval) +{ + return 1; +} + +int pthread_cancel(pthread_t thread) +{ + return 1; +} +int pthread_attr_init(pthread_attr_t *attr) +{ + return 1; +} + +int pthread_attr_setstacksize(pthread_attr_t *attr, size_t stacksize) +{ + return 1; +} + +int pthread_attr_getstacksize(const pthread_attr_t *attr, size_t *stacksize) +{ + return 1; +} + +int pthread_attr_setschedparam(pthread_attr_t *attr, const struct sched_param *param) +{ + return 1; +} + +int pthread_create(pthread_t *thread, const pthread_attr_t *attr, void *(*start_routine)(void *), void *arg) +{ + return 1; +} +int pthread_attr_getschedparam(const pthread_attr_t *attr, struct sched_param *param) +{ + return 1; +} + +int pthread_attr_destroy(pthread_attr_t *attr) +{ + return 1; +} + +int clock_gettime(clockid_t clk_id, struct timespec *tp) +{ + return 1; +} diff --git a/src/platforms/qurt/stubs/stubs_qurt.c b/src/platforms/qurt/stubs/stubs_qurt.c new file mode 100644 index 000000000000..f114791c4dc4 --- /dev/null +++ b/src/platforms/qurt/stubs/stubs_qurt.c @@ -0,0 +1,16 @@ +#include +#include +#include + +void HAP_debug(const char *msg, int level, const char *filename, int line) +{ +} + +void HAP_power_request(int a, int b, int c) +{ +} + +int dlinit(int a, char **b) +{ + return 1; +} diff --git a/src/platforms/qurt/tests/hello/CMakeLists.txt b/src/platforms/qurt/tests/hello/CMakeLists.txt new file mode 100644 index 000000000000..4664b35f8999 --- /dev/null +++ b/src/platforms/qurt/tests/hello/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__qurt__tests__hello + MAIN hello + SRCS + hello_main.cpp + hello_start_qurt.cpp + hello_example.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/qurt/tests/hello/hello_example.cpp b/src/platforms/qurt/tests/hello/hello_example.cpp index 1cfcf463e123..09c356b53fc0 100644 --- a/src/platforms/qurt/tests/hello/hello_example.cpp +++ b/src/platforms/qurt/tests/hello/hello_example.cpp @@ -49,8 +49,9 @@ int HelloExample::main() { appState.setRunning(true); - int i=0; - while (!appState.exitRequested() && i<5) { + int i = 0; + + while (!appState.exitRequested() && i < 5) { PX4_DEBUG(" Doing work..."); ++i; diff --git a/src/platforms/qurt/tests/hello/hello_example.h b/src/platforms/qurt/tests/hello/hello_example.h index a4ae51705637..bf589996d1a3 100644 --- a/src/platforms/qurt/tests/hello/hello_example.h +++ b/src/platforms/qurt/tests/hello/hello_example.h @@ -41,7 +41,8 @@ #include -class HelloExample { +class HelloExample +{ public: HelloExample() {}; diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp index e4180307ce31..56aa52b4785e 100644 --- a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp +++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp @@ -71,11 +71,11 @@ int hello_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("hello", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 16000, - PX4_MAIN, - (char* const*)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char *const *)argv); return 0; } diff --git a/src/platforms/qurt/tests/muorb/CMakeLists.txt b/src/platforms/qurt/tests/muorb/CMakeLists.txt new file mode 100644 index 000000000000..ac233a12022d --- /dev/null +++ b/src/platforms/qurt/tests/muorb/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE platforms__qurt__tests__muorb + MAIN muorb_test + SRCS + muorb_test_start_qurt.cpp + muorb_test_example.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/qurt/tests/muorb/module.mk b/src/platforms/qurt/tests/muorb/module.mk new file mode 100644 index 000000000000..e21ee3c60b0f --- /dev/null +++ b/src/platforms/qurt/tests/muorb/module.mk @@ -0,0 +1,48 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = muorb_test + +SRCS = \ + muorb_test_start_qurt.cpp \ + muorb_test_example.cpp + +INCLUDE_DIRS += $(PX4_BASE)/src/modules/uORB \ + $(PX4_BASE)/src/platforms \ + $(PX4_BASE)/src/modules + + diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp new file mode 100644 index 000000000000..3b932b5f13b4 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 muorb_test_example.cpp + * Example for muorb + * + * @author Mark Charlebois + */ + +#include "muorb_test_example.h" +#include +#include +#include +#include +#include "uORB/topics/sensor_combined.h" +#include "uORB/topics/pwm_input.h" +#include "modules/uORB/uORB.h" +#include "px4_middleware.h" +#include "px4_defines.h" +#include +#include + +px4::AppState MuorbTestExample::appState; + +int MuorbTestExample::main() +{ + int rc; + appState.setRunning(true); + rc = PingPongTest(); + rc = FileReadTest(); + rc = uSleepTest(); + appState.setRunning(false); + return rc; +} + +int MuorbTestExample::DefaultTest() +{ + struct pwm_input_s pwm; + struct sensor_combined_s sc; + + memset(&pwm, 0, sizeof(pwm_input_s)); + memset(&sc, 0, sizeof(sensor_combined_s)); + PX4_WARN("Successful after memset... "); + orb_advert_t pub_fd = orb_advertise(ORB_ID(pwm_input), &pwm); + + if (pub_fd == nullptr) { + PX4_WARN("Error: advertizing pwm_input topic"); + return -1; + } + + orb_advert_t pub_sc = orb_advertise(ORB_ID(sensor_combined), &sc); + + if (pub_sc == nullptr) { + PX4_WARN("Error: advertizing sensor_combined topic"); + return -1; + } + + int i = 0; + pwm.error_count++; + sc.gyro_errcount++; + + while (!appState.exitRequested() && i < 10) { + + PX4_INFO(" Doing work..."); + orb_publish(ORB_ID(pwm_input), pub_fd, &pwm); + orb_publish(ORB_ID(sensor_combined), pub_sc, &sc); + + usleep(1000000); + ++i; + } + + return 0; +} + +int MuorbTestExample::PingPongTest() +{ + int i = 0; + orb_advert_t pub_id_esc_status = orb_advertise(ORB_ID(esc_status), & m_esc_status); + + if (pub_id_esc_status == 0) { + PX4_ERR("error publishing esc_status"); + return -1; + } + + if (orb_publish(ORB_ID(esc_status), pub_id_esc_status, &m_esc_status) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the esc_status message", i); + return -1; + } + + int sub_vc = orb_subscribe(ORB_ID(vehicle_command)); + + if (sub_vc == PX4_ERROR) { + PX4_ERR("Error subscribing to vehicle_command topic"); + return -1; + } + + while (!appState.exitRequested()) { + + PX4_DEBUG("[%d] Doing work...", i); + bool updated = false; + + if (orb_check(sub_vc, &updated) == 0) { + if (updated) { + PX4_WARN("[%d]vechile command status is updated... reading new value", i); + + if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) { + PX4_ERR("[%d]Error calling orb copy for vechicle... ", i); + break; + } + + if (orb_publish(ORB_ID(esc_status), pub_id_esc_status, &m_esc_status) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the esc_status message", i); + break; + } + + } else { + PX4_WARN("[%d] vechicle command topic is not updated", i); + } + + } else { + PX4_ERR("[%d]Error checking the updated status for vehicle command ", i); + break; + } + + // sleep for 1 sec. + usleep(1000000); + + ++i; + } + + return 0; +} + +int MuorbTestExample::uSleepTest() +{ + PX4_WARN("before usleep for 1 sec [%" PRIu64 "]", hrt_absolute_time()); + usleep(1000000); + PX4_INFO("After usleep for 1 sec [%" PRIu64 "]", hrt_absolute_time()); + + for (int i = 0; i < 10; ++i) { + PX4_INFO("In While Loop: B4 Sleep for[%d] seconds [%" PRIu64 "]" , i + 1, hrt_absolute_time()); + usleep((i + 1) * 1000000); + PX4_INFO("In While Loop: After Sleep for[%d] seconds [%" PRIu64 "]" , i + 1 , hrt_absolute_time()); + } + + PX4_INFO("exiting sleep test..."); + return 0; +} + +int MuorbTestExample::FileReadTest() +{ + int rc = OK; + static const char TEST_FILE_PATH[] = "./test.txt"; + FILE *fp; + char *line = NULL; + size_t len = 0; + ssize_t read; + + fp = fopen(TEST_FILE_PATH, "r"); + + if (fp == NULL) { + PX4_WARN("unable to open file[%s] for reading", TEST_FILE_PATH); + rc = PX4_ERROR; + + } else { + /* + int i = 0; + while( ( read = getline( &line, &len, fp ) ) != -1 ) + { + ++i; + PX4_INFO( "LineNum[%d] LineLength[%d]", i, len ); + PX4_INFO( "LineNum[%d] Line[%s]", i, line ); + } + */ + PX4_INFO("Successfully opened file [%s]", TEST_FILE_PATH); + fclose(fp); + + if (line != NULL) { + free(line); + } + } + + return rc; +} diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.h b/src/platforms/qurt/tests/muorb/muorb_test_example.h new file mode 100644 index 000000000000..68236eb854ea --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 + +#include +#include "uORB/topics/esc_status.h" +#include "uORB/topics/vehicle_command.h" + +class MuorbTestExample +{ +public: + MuorbTestExample() {}; + ~MuorbTestExample() {}; + + int main(); + + static px4::AppState appState; /* track requests to terminate app */ + +private: + int DefaultTest(); + int PingPongTest(); + int FileReadTest(); + int uSleepTest(); + + struct esc_status_s m_esc_status; + struct vehicle_command_s m_vc; + +}; diff --git a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp new file mode 100644 index 000000000000..c6818a9270fb --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 muorb_test_main.cpp + * Test of Multi-uORB supoprt + * + * @author Mark Charlebois + */ +#include +#include +#include +#include "muorb_test_example.h" + +extern "C" __EXPORT int muorb_test_entry(int argc, char **argv); + +int muorb_test_entry(int argc, char **argv) +{ + px4::init(argc, argv, "muorb_test"); + + PX4_DEBUG("muorb_test"); + MuorbTestExample hello; + hello.main(); + + PX4_DEBUG("goodbye"); + return 0; +} diff --git a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp new file mode 100644 index 000000000000..ba44169ec3a0 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 muorb_test_start_qurt.cpp + * + * @author Mark Charlebois + */ +#include "muorb_test_example.h" +#include +#include +#include +#include +#include +#include + +static int daemon_task; /* Handle of deamon task / thread */ + +//using namespace px4; + +extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]); + +int muorb_test_entry(int argc, char **argv) +{ + //px4::init(argc, argv, "muorb_test"); + + PX4_INFO("muorb_test entry....."); + MuorbTestExample hello; + hello.main(); + + PX4_INFO("goodbye"); + return 0; +} + +static void usage() +{ + PX4_DEBUG("usage: muorb_test {start|stop|status}"); +} +int muorb_test_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("already running"); + /* this is not an error */ + return 0; + } + + PX4_INFO("before starting the muorb_test_entry task"); + + daemon_task = px4_task_spawn_cmd("muorb_test", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 8192, + muorb_test_entry, + (char *const *)argv); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + MuorbTestExample::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("is running"); + + } else { + PX4_DEBUG("not started"); + } + + return 0; + } + + usage(); + return 1; +} diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp index 2f277d7f167f..d13a5e277a82 100644 --- a/src/platforms/ros/geo.cpp +++ b/src/platforms/ros/geo.cpp @@ -182,7 +182,8 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference return ref->timestamp; } -__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { ref->lat_rad = lat_0 * M_DEG_TO_RAD; @@ -196,12 +197,14 @@ __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s * return 0; } -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, + double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros()); } -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad) { if (!map_projection_initialized(ref)) { return -1; @@ -213,7 +216,8 @@ __EXPORT int map_projection_reference(const struct map_projection_reference_s *r return 0; } -__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y) { if (!map_projection_initialized(ref)) { return -1; @@ -235,7 +239,8 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref return 0; } -__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon) { if (!map_projection_initialized(ref)) { return -1; diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index abaa6fc60d76..38db5dad7f4a 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -109,44 +109,45 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon } void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, - px4::vehicle_control_mode &msg_vehicle_control_mode) + px4::vehicle_control_mode &msg_vehicle_control_mode) { msg_vehicle_control_mode.flag_control_manual_enabled = false; msg_vehicle_control_mode.flag_control_offboard_enabled = true; msg_vehicle_control_mode.flag_control_auto_enabled = false; msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate || - !msg_offboard_control_mode.ignore_attitude || - !msg_offboard_control_mode.ignore_position || - !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_acceleration_force; + !msg_offboard_control_mode.ignore_attitude || + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude || - !msg_offboard_control_mode.ignore_position || - !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_acceleration_force; + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_position; + !msg_offboard_control_mode.ignore_position; msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_position; + !msg_offboard_control_mode.ignore_position; msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position; - msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position; + msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_position; } void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, - px4::vehicle_status &msg_vehicle_status, - px4::vehicle_control_mode &msg_vehicle_control_mode) { + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode) +{ // XXX this is a minimal implementation. If more advanced functionalities are // needed consider a full port of the commander - if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) - { + if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) { SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode); return; } @@ -154,7 +155,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_offboard_enabled = false; switch (msg->mode_switch) { - case px4::manual_control_setpoint::SWITCH_POS_NONE: + case px4::manual_control_setpoint::SWITCH_POS_NONE: ROS_WARN("Joystick button mapping error, main mode not set"); break; @@ -182,6 +183,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; msg_vehicle_control_mode.flag_control_position_enabled = true; msg_vehicle_control_mode.flag_control_velocity_enabled = true; + } else { msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL; msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL; @@ -193,6 +195,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_position_enabled = false; msg_vehicle_control_mode.flag_control_velocity_enabled = false; } + break; } @@ -205,20 +208,20 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons /* Force system into offboard control mode */ if (!_got_manual_control) { SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); - + _msg_vehicle_status.timestamp = px4::get_time_micros(); _msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF; _msg_vehicle_status.hil_state = _msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; _msg_vehicle_status.is_rotary_wing = true; _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED; - + _msg_actuator_armed.armed = true; _msg_actuator_armed.timestamp = px4::get_time_micros(); _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); _msg_vehicle_control_mode.flag_armed = true; - + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index 856144389d83..67bee544ab95 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -68,14 +68,14 @@ class Commander * Set control mode flags based on stick positions (equiv to code in px4 commander) */ void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, - px4::vehicle_status &msg_vehicle_status, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode); /** * Sets offboard controll flags in msg_vehicle_control_mode */ void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_control_mode &msg_vehicle_control_mode); ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp index 328f545c6b80..7b02a6f07ef2 100644 --- a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -65,15 +65,18 @@ int DemoOffboardAttitudeSetpoints::main() /* Publish example offboard attitude setpoint */ geometry_msgs::PoseStamped pose; - tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , 0.0); + tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , + 0.0); quaternionTFToMsg(q, pose.pose.orientation); _attitude_sp_pub.publish(pose); std_msgs::Float64 thrust; - thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump' + thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / + 1000000.0f)); // just some example throttle input that makes the quad 'jump' _thrust_sp_pub.publish(thrust); } + return 0; } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 8488c518f5a6..fc8db220c3ee 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -116,12 +116,14 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do if (val + offset > deadzone || val + offset < -deadzone) { out = (float)((val + offset)) * scale; + } else { out = 0.0f; } } -void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) { +void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) +{ msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) { @@ -150,6 +152,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; + } else if (_buttons_state[MAIN_STATE_OFFBOARD] != msg->buttons[_param_buttons_map[MAIN_STATE_OFFBOARD]] == true) { msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 2758979a29b8..e0152668095a 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -46,7 +46,7 @@ using namespace px4; -Mavlink::Mavlink() : +Mavlink::Mavlink(std::string mavlink_fcu_url) : _n(), _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), _v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)), @@ -55,7 +55,7 @@ Mavlink::Mavlink() : _offboard_control_mode_pub(_n.advertise("offboard_control_mode", 1)), _force_sp_pub(_n.advertise("vehicle_force_setpoint", 1)) { - _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); + _link = mavconn::MAVConnInterface::open_url(mavlink_fcu_url); _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); _att_sp = {}; _offboard_control_mode = {}; @@ -64,7 +64,11 @@ Mavlink::Mavlink() : int main(int argc, char **argv) { ros::init(argc, argv, "mavlink"); - Mavlink m; + ros::NodeHandle privateNh("~"); + std::string mavlink_fcu_url; + privateNh.param("mavlink_fcu_url", mavlink_fcu_url, + std::string("udp://localhost:14565@localhost:14560")); + Mavlink m(mavlink_fcu_url); ros::spin(); return 0; } @@ -73,18 +77,18 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) { mavlink_message_t msg_m; mavlink_msg_attitude_quaternion_pack_chan( - _link->get_system_id(), - _link->get_component_id(), - _link->get_channel(), - &msg_m, - get_time_micros() / 1000, - msg->q[0], - msg->q[1], - msg->q[2], - msg->q[3], - msg->rollspeed, - msg->pitchspeed, - msg->yawspeed); + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->q[0], + msg->q[1], + msg->q[2], + msg->q[3], + msg->rollspeed, + msg->pitchspeed, + msg->yawspeed); _link->send_message(&msg_m); } @@ -92,33 +96,36 @@ void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr { mavlink_message_t msg_m; mavlink_msg_local_position_ned_pack_chan( - _link->get_system_id(), - _link->get_component_id(), - _link->get_channel(), - &msg_m, - get_time_micros() / 1000, - msg->x, - msg->y, - msg->z, - msg->vx, - msg->vy, - msg->vz); + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->x, + msg->y, + msg->z, + msg->vx, + msg->vy, + msg->vz); _link->send_message(&msg_m); } -void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) { +void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) +{ (void)sysid; (void)compid; - switch(mmsg->msgid) { - case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: - handle_msg_set_attitude_target(mmsg); - break; - case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: - handle_msg_set_position_target_local_ned(mmsg); - break; - default: - break; + switch (mmsg->msgid) { + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_msg_set_attitude_target(mmsg); + break; + + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_msg_set_position_target_local_ned(mmsg); + break; + + default: + break; } } @@ -143,10 +150,12 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; + } else { _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; _offboard_control_mode.ignore_attitude = ignore_attitude; } + _offboard_control_mode.ignore_position = true; _offboard_control_mode.ignore_velocity = true; _offboard_control_mode.ignore_acceleration_force = true; @@ -159,13 +168,14 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) */ _att_sp.timestamp = get_time_micros(); + if (!ignore_attitude) { mavlink_quaternion_to_euler(set_attitude_target.q, &_att_sp.roll_body, &_att_sp.pitch_body, - &_att_sp.yaw_body); + &_att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body.data()); _att_sp.R_valid = true; } - + if (!_offboard_control_mode.ignore_thrust) { _att_sp.thrust = set_attitude_target.thrust; @@ -175,6 +185,7 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) for (ssize_t i = 0; i < 4; i++) { _att_sp.q_d[i] = set_attitude_target.q[i]; } + _att_sp.q_d_valid = true; } @@ -197,9 +208,9 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * /* Only accept messages which are intended for this system */ // XXX removed for sitl, makes maybe sense to re-introduce at some point // if ((mavlink_system.sysid == set_position_target_local_ned.target_system || - // set_position_target_local_ned.target_system == 0) && - // (mavlink_system.compid == set_position_target_local_ned.target_component || - // set_position_target_local_ned.target_component == 0)) { + // set_position_target_local_ned.target_system == 0) && + // (mavlink_system.compid == set_position_target_local_ned.target_component || + // set_position_target_local_ned.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); @@ -220,7 +231,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * * gets published only if in offboard mode. We leave that out for now. */ if (is_force_sp && offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity) { + offboard_control_mode.ignore_velocity) { /* The offboard setpoint is a force setpoint only, directly writing to the force * setpoint topic and not publishing the setpoint triplet topic */ vehicle_force_setpoint force_sp; @@ -230,6 +241,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * //XXX: yaw _force_sp_pub.publish(force_sp); + } else { /* It's not a pure force setpoint: publish to setpoint triplet topic */ position_setpoint_triplet pos_sp_triplet; @@ -244,6 +256,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * pos_sp_triplet.current.x = set_position_target_local_ned.x; pos_sp_triplet.current.y = set_position_target_local_ned.y; pos_sp_triplet.current.z = set_position_target_local_ned.z; + } else { pos_sp_triplet.current.position_valid = false; } @@ -254,6 +267,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * pos_sp_triplet.current.vx = set_position_target_local_ned.vx; pos_sp_triplet.current.vy = set_position_target_local_ned.vy; pos_sp_triplet.current.vz = set_position_target_local_ned.vz; + } else { pos_sp_triplet.current.velocity_valid = false; } @@ -273,7 +287,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * } /* set the yaw sp value */ - if (!offboard_control_mode.ignore_attitude) { + if (!offboard_control_mode.ignore_attitude && !isnan(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yaw_valid = true; pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; @@ -282,13 +296,14 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * } /* set the yawrate sp value */ - if (!offboard_control_mode.ignore_bodyrate) { + if (!offboard_control_mode.ignore_bodyrate && !isnan(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; } else { pos_sp_triplet.current.yawspeed_valid = false; } + //XXX handle global pos setpoints (different MAV frames) _pos_sp_triplet_pub.publish(pos_sp_triplet); diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 8b7d08d24274..af9e01999379 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -55,7 +55,7 @@ namespace px4 class Mavlink { public: - Mavlink(); + Mavlink(std::string mavlink_fcu_url); ~Mavlink() {} diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 002a112b69b4..67084c64bb01 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -221,14 +221,19 @@ void MultirotorMixer::actuatorControlsCallback(const px4::actuator_controls_0 &m // switch mixer if necessary std::string mixer_name; _n.getParamCached("mixer", mixer_name); + if (mixer_name == "x") { _rotors = _config_index[0]; + } else if (mixer_name == "+") { _rotors = _config_index[1]; + } else if (mixer_name == "e") { _rotors = _config_index[2]; + } else if (mixer_name == "w") { _rotors = _config_index[3]; + } else if (mixer_name == "i") { _rotors = _config_index[4]; } diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp index e4273687e7f1..d1861ca1815f 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -54,6 +54,7 @@ PositionEstimator::PositionEstimator() : _vehicle_position_pub(_n.advertise("vehicle_local_position", 1)), _startup_time(1) { + _n.getParam("vehicle_model", _model_name); } void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) @@ -68,7 +69,7 @@ void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP //XXX: maybe a more clever approach would be to do this not on every loop, need to check if and when //gazebo rearranges indexes. for (std::vector::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) { - if (*it == "iris" || *it == "ardrone") { + if (*it == _model_name) { index = it - msg->name.begin(); break; } diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h index da1fc2c5a8b1..ee46cdf77d57 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.h +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h @@ -57,6 +57,6 @@ class PositionEstimator ros::Publisher _vehicle_position_pub; uint64_t _startup_time; - + std::string _model_name; }; From ff386198a15f98a79e7e75a0e10f64724588388f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:12:43 +1100 Subject: [PATCH 230/293] drivers: added missing cmake files this keeps diffs small --- src/drivers/ll40ls/CMakeLists.txt | 46 +++++++++++++++++++++++ src/drivers/lsm303d/CMakeLists.txt | 45 +++++++++++++++++++++++ src/drivers/mpu6000/CMakeLists.txt | 45 +++++++++++++++++++++++ src/drivers/ms5611/CMakeLists.txt | 59 ++++++++++++++++++++++++++++++ 4 files changed, 195 insertions(+) create mode 100644 src/drivers/ll40ls/CMakeLists.txt create mode 100644 src/drivers/lsm303d/CMakeLists.txt create mode 100644 src/drivers/mpu6000/CMakeLists.txt create mode 100644 src/drivers/ms5611/CMakeLists.txt diff --git a/src/drivers/ll40ls/CMakeLists.txt b/src/drivers/ll40ls/CMakeLists.txt new file mode 100644 index 000000000000..e8f6a9af969e --- /dev/null +++ b/src/drivers/ll40ls/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__ll40ls + MAIN ll40ls + COMPILE_FLAGS + -Os + SRCS + ll40ls.cpp + LidarLite.cpp + LidarLiteI2C.cpp + LidarLitePWM.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/lsm303d/CMakeLists.txt b/src/drivers/lsm303d/CMakeLists.txt new file mode 100644 index 000000000000..c781592137e2 --- /dev/null +++ b/src/drivers/lsm303d/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__lsm303d + MAIN lsm303d + STACK 1200 + COMPILE_FLAGS + -Weffc++ + -Os + SRCS + lsm303d.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/mpu6000/CMakeLists.txt b/src/drivers/mpu6000/CMakeLists.txt new file mode 100644 index 000000000000..1a1e7a639e86 --- /dev/null +++ b/src/drivers/mpu6000/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__mpu6000 + MAIN mpu6000 + STACK 1200 + COMPILE_FLAGS + -Weffc++ + -Os + SRCS + mpu6000.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/ms5611/CMakeLists.txt b/src/drivers/ms5611/CMakeLists.txt new file mode 100644 index 000000000000..7359bd9507f3 --- /dev/null +++ b/src/drivers/ms5611/CMakeLists.txt @@ -0,0 +1,59 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +set(srcs + ms5611_spi.cpp + ms5611_i2c.cpp + ) + +if(${OS} STREQUAL "nuttx") + list(APPEND srcs + ms5611_nuttx.cpp + ) +else() + list(APPEND srcs + ms5611_posix.cpp + ms5611_sim.cpp + ) + +endif() + +px4_add_module( + MODULE drivers__ms5611 + MAIN ms5611 + COMPILE_FLAGS + -Os + SRCS ${srcs} + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : From c7d718df441916eaffb31baf7a99bc54c1d42149 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 10:58:05 +1100 Subject: [PATCH 231/293] px4iofirmware: update from upstream --- src/modules/px4iofirmware/adc.c | 9 +- src/modules/px4iofirmware/controls.c | 104 +++++--- src/modules/px4iofirmware/i2c.c | 50 ++-- src/modules/px4iofirmware/mixer.cpp | 206 ++++++++++------ src/modules/px4iofirmware/protocol.h | 22 +- src/modules/px4iofirmware/px4io.c | 56 +++-- src/modules/px4iofirmware/px4io.h | 14 +- src/modules/px4iofirmware/registers.c | 332 ++++++++++++++++---------- src/modules/px4iofirmware/safety.c | 4 +- src/modules/px4iofirmware/serial.c | 40 +++- 10 files changed, 529 insertions(+), 308 deletions(-) diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index d3db26c233cc..1688d53048d6 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -96,19 +96,22 @@ adc_init(void) rCR2 |= ADC_CR2_RSTCAL; up_udelay(1); - if (rCR2 & ADC_CR2_RSTCAL) + if (rCR2 & ADC_CR2_RSTCAL) { return -1; + } rCR2 |= ADC_CR2_CAL; up_udelay(100); - if (rCR2 & ADC_CR2_CAL) + if (rCR2 & ADC_CR2_CAL) { return -1; + } + #endif /* * Configure sampling time. - * + * * For electrical protection reasons, we want to be able to have * 10K in series with ADC inputs that leave the board. At 12MHz this * means we need 28.5 cycles of sampling time (per table 43 in the diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index a0722ac4d6fc..25874868b1d3 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -46,6 +46,8 @@ #include #include #include +#include +#include #include "px4io.h" @@ -60,10 +62,15 @@ static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; static perf_counter_t c_gather_ppm; -static int _dsm_fd; +static int _dsm_fd = -1; +int _sbus_fd = -1; static uint16_t rc_value_override = 0; +#ifdef ADC_RSSI +static unsigned _rssi_adc_counts = 0; +#endif + bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { perf_begin(c_gather_dsm); @@ -71,17 +78,22 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool uint8_t n_bytes = 0; uint8_t *bytes; *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + if (*dsm_updated) { r_raw_rc_count = temp_count & 0x7fff; - if (temp_count & 0x8000) + + if (temp_count & 0x8000) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - else + + } else { r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + } r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } + perf_end(c_gather_dsm); /* get data from FD and attempt to parse with DSM and ST24 libs */ @@ -94,7 +106,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool /* 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)); + &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*st24_updated) { @@ -121,7 +133,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool /* set updated flag if one complete packet was parsed */ sumd_rssi = RC_INPUT_RSSI_MAX; *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, - &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); + &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*sumd_updated) { @@ -149,7 +161,7 @@ controls_init(void) _dsm_fd = dsm_init("/dev/ttyS0"); /* S.bus input (USART3) */ - sbus_init("/dev/ttyS2"); + _sbus_fd = sbus_init("/dev/ttyS2", false); /* default to a 1:1 input map, all enabled */ for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) { @@ -170,7 +182,8 @@ controls_init(void) } void -controls_tick() { +controls_tick() +{ /* * Gather R/C control inputs from supported sources. @@ -184,19 +197,24 @@ controls_tick() { uint16_t rssi = 0; #ifdef ADC_RSSI + if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { - /* use 1:1 scaling on 3.3V ADC input */ - unsigned mV = counts * 3300 / 4096; + /* low pass*/ + _rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f); + /* use 1:1 scaling on 3.3V, 12-Bit ADC input */ + unsigned mV = _rssi_adc_counts * 3300 / 4095; + /* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */ + rssi = (mV * RC_INPUT_RSSI_MAX / 3300); - /* scale to 0..253 and lowpass */ - rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f); if (rssi > RC_INPUT_RSSI_MAX) { rssi = RC_INPUT_RSSI_MAX; } } } + #endif /* zero RSSI if signal is lost */ @@ -207,21 +225,26 @@ controls_tick() { perf_begin(c_gather_dsm); bool dsm_updated, st24_updated, sumd_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_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; } + if (sumd_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; } + perf_end(c_gather_dsm); perf_begin(c_gather_sbus); bool sbus_failsafe, sbus_frame_drop; - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); + bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; @@ -231,17 +254,19 @@ controls_tick() { if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; sbus_rssi = RC_INPUT_RSSI_MAX / 2; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } if (sbus_failsafe) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } - /* set RSSI to an emulated value if ADC RSSI is off */ + /* set RSSI to an emulated value if ADC RSSI is off */ if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { rssi = sbus_rssi; } @@ -257,17 +282,20 @@ controls_tick() { */ perf_begin(c_gather_ppm); bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); + if (ppm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } + perf_end(c_gather_ppm); /* limit number of channels to allowable data size */ - if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) + if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) { r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; + } /* store RSSI */ r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; @@ -308,10 +336,13 @@ controls_tick() { /* * 1) Constrain to min/max values, as later processing depends on bounds. */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) { raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + } + + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) { raw = conf[PX4IO_P_RC_CONFIG_MAX]; + } /* * 2) Scale around the mid point differently for lower and upper range. @@ -330,10 +361,12 @@ controls_tick() { * DO NOT REMOVE OR ALTER STEP 1! */ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( + conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( + conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); } else { /* in the configured dead zone, output zero */ @@ -347,6 +380,7 @@ controls_tick() { /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { /* invert channel if pitch - pulling the lever down means pitching up by convention */ @@ -360,6 +394,7 @@ controls_tick() { if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } @@ -389,6 +424,7 @@ controls_tick() { /* if we have enough channels (5) to control the vehicle, the mapping is ok */ if (assigned_channels > 4) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } @@ -408,9 +444,9 @@ controls_tick() { /* clear the input-kind flags here */ r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_RC_PPM | - PX4IO_P_STATUS_FLAGS_RC_DSM | - PX4IO_P_STATUS_FLAGS_RC_SBUS); + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS); } @@ -427,8 +463,8 @@ controls_tick() { if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_OVERRIDE | - PX4IO_P_STATUS_FLAGS_RC_OK); + PX4IO_P_STATUS_FLAGS_OVERRIDE | + PX4IO_P_STATUS_FLAGS_RC_OK); /* flag raw RC as lost */ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); @@ -451,9 +487,9 @@ controls_tick() { * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; @@ -464,8 +500,9 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) + 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 @@ -473,20 +510,23 @@ controls_tick() { */ 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; + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + override = true; + } if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ - if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) + if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) { mixer_tick(); + } } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } + } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } @@ -512,8 +552,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; - if (*num_values > PX4IO_RC_INPUT_CHANNELS) + + if (*num_values > PX4IO_RC_INPUT_CHANNELS) { *num_values = PX4IO_RC_INPUT_CHANNELS; + } for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) { values[i] = ppm_buffer[i]; diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 6d1d1fc2dd5b..74692a2675c0 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -120,7 +120,7 @@ interface_init(void) rCR1 = 0; /* set for DMA operation */ - rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + rCR2 |= I2C_CR2_ITEVFEN | I2C_CR2_ITERREN | I2C_CR2_DMAEN; /* set the frequency value in CR2 */ rCR2 &= ~I2C_CR2_FREQ_MASK; @@ -128,8 +128,11 @@ interface_init(void) /* set divisor and risetime for fast mode */ uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); - if (result < 1) + + if (result < 1) { result = 1; + } + result = 3; rCCR &= ~I2C_CCR_CCR_MASK; rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; @@ -163,7 +166,7 @@ i2c_reset(void) rCR1 = 0; /* set for DMA operation */ - rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + rCR2 |= I2C_CR2_ITEVFEN | I2C_CR2_ITERREN | I2C_CR2_DMAEN; /* set the frequency value in CR2 */ rCR2 &= ~I2C_CR2_FREQ_MASK; @@ -171,8 +174,11 @@ i2c_reset(void) /* set divisor and risetime for fast mode */ uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); - if (result < 1) + + if (result < 1) { result = 1; + } + result = 3; rCCR &= ~I2C_CCR_CCR_MASK; rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; @@ -203,13 +209,16 @@ i2c_interrupt(int irq, FAR void *context) case DIR_TX: i2c_tx_complete(); break; + case DIR_RX: i2c_rx_complete(); break; + default: /* not currently transferring - must be a new txn */ break; } + direction = DIR_NONE; } @@ -232,8 +241,9 @@ i2c_interrupt(int irq, FAR void *context) } /* clear any errors that might need it (this handles AF as well */ - if (sr1 & I2C_SR1_ERRORMASK) + if (sr1 & I2C_SR1_ERRORMASK) { rSR1 = 0; + } return 0; } @@ -248,13 +258,13 @@ i2c_rx_setup(void) */ rx_len = 0; stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf), - DMA_CCR_CIRC | - DMA_CCR_MINC | - DMA_CCR_PSIZE_32BITS | - DMA_CCR_MSIZE_8BITS | - DMA_CCR_PRIMED); + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_32BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); - stm32_dmastart(rx_dma, NULL, NULL, false); + stm32_dmastart(rx_dma, NULL, NULL, false); } static void @@ -269,8 +279,10 @@ i2c_rx_complete(void) /* work out how many registers are being written */ unsigned count = (rx_len - 2) / 2; + if (count > 0) { registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count); + } else { /* no registers written, must be an address cycle */ uint16_t *regs; @@ -278,9 +290,11 @@ i2c_rx_complete(void) /* work out which registers are being addressed */ int ret = registers_get(selected_page, selected_offset, ®s, ®_count); + if (ret == 0) { tx_buf = (uint8_t *)regs; tx_len = reg_count * 2; + } else { tx_buf = junk_buf; tx_len = sizeof(junk_buf); @@ -306,16 +320,16 @@ i2c_tx_setup(void) /* * Note that we configure DMA in circular mode; this means that a too-long * transfer will copy the buffer more than once, but that avoids us having - * to deal with bailing out of a transaction while the master is still + * to deal with bailing out of a transaction while the master is still * babbling at us. */ stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len, - DMA_CCR_DIR | - DMA_CCR_CIRC | - DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS | - DMA_CCR_PRIMED); + DMA_CCR_DIR | + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); stm32_dmastart(tx_dma, NULL, NULL, false); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 4fc0fe938a6f..4ff95bd5afe3 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ * @file mixer.cpp * * Control channel input/output mixer and failsafe. + * + * @author Lorenz Meier */ #include @@ -46,9 +48,11 @@ #include #include +#include #include #include +#include extern "C" { //#define DEBUG @@ -59,13 +63,17 @@ extern "C" { * Maximum interval in us before FMU signal is considered lost */ #define FMU_INPUT_DROP_LIMIT_US 500000 +#define NAN_VALUE (0.0f/0.0f) /* current servo arm/disarm state */ static bool mixer_servos_armed = false; static bool should_arm = false; +static bool should_arm_nothrottle = false; static bool should_always_enable_pwm = false; static volatile bool in_mixer = false; +extern int _sbus_fd; + /* selected control values and count for mixing */ enum mixer_source { MIX_NONE, @@ -83,21 +91,19 @@ static int mixer_callback(uintptr_t handle, static MixerGroup mixer_group(mixer_callback, 0); -/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ -static void mixer_set_failsafe(); - void mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ if ((system_state.fmu_data_received_time == 0) || - hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; @@ -115,42 +121,39 @@ mixer_tick(void) * Decide which set of controls we're using. */ - bool override_enabled = ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)); - /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + /* don't actually mix anything - we already have raw PWM values */ + source = MIX_NONE; - if (override_enabled) { - /* a channel based override has been - * triggered, with FMU active */ - source = MIX_OVERRIDE_FMU_OK; - } else { - /* don't actually mix anything - we already have raw PWM values */ - source = MIX_NONE; - } } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ source = MIX_FMU; } - if ( override_enabled && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - /* do not enter manual override if we asked for termination failsafe and FMU is lost */ - !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + /* do not enter manual override if we asked for termination failsafe and FMU is lost */ + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { - /* if allowed, mix from RC inputs directly */ + /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; - } else if ( override_enabled && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + } else if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly up to available rc channels */ source = MIX_OVERRIDE_FMU_OK; @@ -165,26 +168,32 @@ mixer_tick(void) * */ should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) - ) - ); + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) + ) + ); + + should_arm_nothrottle = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); /* * Check if failsafe termination is set - if yes, * set the force failsafe flag once entering the first * failsafe condition. */ - if ( /* if we have requested flight termination style failsafe (noreturn) */ + if ( /* if we have requested flight termination style failsafe (noreturn) */ (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && /* and we ended up in a failsafe condition */ (source == MIX_FAILSAFE) && @@ -207,6 +216,7 @@ mixer_tick(void) */ if (source == MIX_FAILSAFE) { r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; + } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } @@ -238,26 +248,28 @@ mixer_tick(void) in_mixer = false; /* the pwm limit call takes care of out of band errors */ - pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, + r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) + /* clamp unused outputs to zero */ + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = 0; + outputs[i] = 0.0f; + } + /* store normalized outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); } } /* set arming */ - bool needs_to_arm = (should_arm || should_always_enable_pwm); + bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm); /* check any conditions that prevent arming */ if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { needs_to_arm = false; } - if (!should_arm && !should_always_enable_pwm) { - needs_to_arm = false; - } if (needs_to_arm && !mixer_servos_armed) { /* need to arm, but not armed */ @@ -274,7 +286,7 @@ mixer_tick(void) isr_debug(5, "> PWM disabled"); } - if (mixer_servos_armed && should_arm) { + if (mixer_servos_armed && (should_arm || should_arm_nothrottle)) { /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); @@ -283,22 +295,26 @@ mixer_tick(void) /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { - sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); + } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { - sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); } } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servo_disarmed[i]); + } /* set S.BUS1 or S.BUS2 outputs */ - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) - sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); + } - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) - sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { + sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); + } } } @@ -308,15 +324,17 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - if (control_group >= PX4IO_CONTROL_GROUPS) + if (control_group >= PX4IO_CONTROL_GROUPS) { return -1; + } switch (source) { case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { + if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } + return -1; case MIX_OVERRIDE: @@ -324,17 +342,21 @@ mixer_callback(uintptr_t handle, control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } + return -1; case MIX_OVERRIDE_FMU_OK: + /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; + } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } + return -1; case MIX_FAILSAFE: @@ -343,6 +365,50 @@ mixer_callback(uintptr_t handle, return -1; } + /* apply trim offsets for override channels */ + if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_ROLL) { + control += REG_TO_FLOAT(r_setup_trim_roll); + + } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_PITCH) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + + } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_YAW) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } + } + + /* limit output */ + if (control > 1.0f) { + control = 1.0f; + + } else if (control < -1.0f) { + control = -1.0f; + } + + /* motor spinup phase - lock throttle to zero */ + if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* limit the throttle output to zero during motor spinup, + * as the motors cannot follow any demand yet + */ + control = 0.0f; + } + } + + /* only safety off, but not armed - set throttle as invalid */ + if (should_arm_nothrottle && !should_arm) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* mark the throttle as invalid */ + control = NAN_VALUE; + } + } + return 0; } @@ -360,7 +426,7 @@ mixer_handle_text(const void *buffer, size_t length) { /* 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)) { + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } @@ -390,7 +456,7 @@ mixer_handle_text(const void *buffer, size_t length) mixer_group.reset(); mixer_text_length = 0; - /* FALLTHROUGH */ + /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); @@ -413,24 +479,14 @@ mixer_handle_text(const void *buffer, size_t length) /* if anything was parsed */ if (resid != mixer_text_length) { - /* only set mixer ok if no residual is left over */ - if (resid == 0) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; - } else { - /* not yet reached the end of the mixer, set as not ok */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - } - isr_debug(2, "used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ - if (resid > 0) + if (resid > 0) { memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + } mixer_text_length = resid; - - /* update failsafe values */ - mixer_set_failsafe(); } break; @@ -439,17 +495,18 @@ mixer_handle_text(const void *buffer, size_t length) return 0; } -static void +void mixer_set_failsafe() { - /* + /* * Check if a custom failsafe value has been written, * or if the mixer is not ok and bail out. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { return; + } /* set failsafe defaults to the values for all inputs = 0 */ float outputs[PX4IO_SERVO_COUNT]; @@ -467,7 +524,8 @@ mixer_set_failsafe() } /* disable the rest of the outputs */ - for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { r_page_servo_failsafe[i] = 0; + } } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index a649920320d4..1267b48853d5 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -33,6 +33,7 @@ #pragma once +#define __STDC_FORMAT_MACROS #include /** @@ -45,7 +46,7 @@ * * The first two bytes of each write select a page and offset address * respectively. Subsequent reads and writes increment the offset within - * the page. + * the page. * * Some pages are read- or write-only. * @@ -55,7 +56,7 @@ * Writes to unimplemented registers are ignored. Reads from unimplemented * registers return undefined values. * - * As convention, values that would be floating point in other parts of + * As convention, values that would be floating point in other parts of * the PX4 system are expressed as signed integer values scaled by 10000, * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and * SIGNED_TO_REG macros to convert between register representation and @@ -65,7 +66,7 @@ * readable pages to be densely packed. Page numbers do not need to be * packed. * - * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, * [2] denotes definitions specific to the PX4IOv2 board. */ @@ -217,14 +218,14 @@ enum { /* DSM bind states */ dsm_bind_send_pulses, dsm_bind_reinit_uart }; - /* 8 */ +/* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ #define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ #define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ - /* storage space of 12 occupied by CRC */ +/* storage space of 12 occupied by CRC */ #define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into 'armed' (PWM enabled) state - this is a non-data write and hence index 12 can safely be used. */ @@ -234,6 +235,9 @@ enum { /* DSM bind states */ #define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ #define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ +#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ @@ -335,8 +339,7 @@ struct IOPacket { #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) #define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) -static const uint8_t crc8_tab[256] __attribute__((unused)) = -{ +static const uint8_t crc8_tab[256] __attribute__((unused)) = { 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, @@ -379,8 +382,9 @@ crc_packet(struct IOPacket *pkt) uint8_t *p = (uint8_t *)pkt; uint8_t c = 0; - while (p < end) - c = crc8_tab[c ^ *(p++)]; + while (p < end) { + c = crc8_tab[c ^ * (p++)]; + } return c; } diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 9f8c3b526d74..1ee50d5a7f64 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -97,11 +97,12 @@ isr_debug(uint8_t level, const char *fmt, ...) if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) { return; } + va_list ap; va_start(ap, fmt); vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap); va_end(ap); - msg_next_in = (msg_next_in+1) % NUM_MSG; + msg_next_in = (msg_next_in + 1) % NUM_MSG; msg_counter++; } @@ -113,11 +114,14 @@ show_debug_messages(void) { if (msg_counter != last_msg_counter) { uint32_t n = msg_counter - last_msg_counter; - if (n > NUM_MSG) n = NUM_MSG; + + if (n > NUM_MSG) { n = NUM_MSG; } + last_msg_counter = msg_counter; + while (n--) { debug("%s", msg[msg_next_out]); - msg_next_out = (msg_next_out+1) % NUM_MSG; + msg_next_out = (msg_next_out + 1) % NUM_MSG; } } } @@ -135,7 +139,7 @@ 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)) { + /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { LED_RING(1); return; } @@ -151,7 +155,7 @@ ring_blink(void) if (brightness_counter < max_brightness) { - bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); + bool on = ((on_counter * 100) / brightness_counter + 1) <= ((brightness * 100) / max_brightness + 1); // XXX once led is PWM driven, // remove the ! in the line below @@ -195,7 +199,7 @@ static uint64_t reboot_time; */ void schedule_reboot(uint32_t time_delta_usec) { - reboot_time = hrt_absolute_time() + time_delta_usec; + reboot_time = hrt_absolute_time() + time_delta_usec; } /** @@ -203,9 +207,9 @@ void schedule_reboot(uint32_t time_delta_usec) */ static void check_reboot(void) { - if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { - up_systemreset(); - } + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } } static void @@ -215,12 +219,14 @@ calculate_fw_crc(void) #define APP_LOAD_ADDRESS 0x08001000 // compute CRC of the current firmware uint32_t sum = 0; + for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum); } + r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF; - r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16; + r_page_setup[PX4IO_P_SETUP_CRC + 1] = sum >> 16; } int @@ -308,7 +314,7 @@ user_start(int argc, char *argv[]) * allocations. We don't want him to be able to * get past that point. This needs to be clearly * documented in the dev guide. - * + * */ if (minfo.mxordblk < 600) { @@ -320,10 +326,12 @@ user_start(int argc, char *argv[]) if (phase) { LED_AMBER(true); LED_BLUE(false); + } else { LED_AMBER(false); LED_BLUE(true); } + up_udelay(250000); phase = !phase; @@ -338,7 +346,8 @@ user_start(int argc, char *argv[]) */ uint64_t last_debug_time = 0; - uint64_t last_heartbeat_time = 0; + uint64_t last_heartbeat_time = 0; + for (;;) { /* track the rate at which the loop is running */ @@ -354,25 +363,14 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); - /* - blink blue LED at 4Hz in normal operation. When in - override blink 4x faster so the user can clearly see - that override is happening. This helps when - pre-flight testing the override system - */ - uint32_t heartbeat_period_us = 250*1000UL; - if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { - heartbeat_period_us /= 4; - } - - if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) { - last_heartbeat_time = hrt_absolute_time(); - heartbeat_blink(); - } + if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } ring_blink(); - check_reboot(); + check_reboot(); /* check for debug activity (default: none) */ show_debug_messages(); @@ -382,7 +380,7 @@ user_start(int argc, char *argv[]) */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 33134b108404..2197b7b7763d 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -113,6 +113,10 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_setup_pwm_reverse r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] +#define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL] +#define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH] +#define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW] + #define r_control_values (&r_page_controls[0]) /* @@ -186,6 +190,8 @@ extern pwm_limit_t pwm_limit; */ extern void mixer_tick(void); extern int mixer_handle_text(const void *buffer, size_t length); +/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ +extern void mixer_set_failsafe(void); /** * Safety switch/LED. @@ -218,14 +224,6 @@ extern uint16_t adc_measure(unsigned channel); */ extern void controls_init(void); extern void controls_tick(void); -extern int dsm_init(const char *device); -extern bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); -extern void dsm_bind(uint16_t cmd, int pulses); -extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, - uint16_t max_channels); -extern void sbus1_output(uint16_t *values, uint16_t num_values); -extern void sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 6acdf6774817..a3351bcbd5bd 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -47,6 +47,7 @@ #include #include #include +#include #include "px4io.h" #include "protocol.h" @@ -112,15 +113,14 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; * * Raw RC input */ -uint16_t r_page_raw_rc_input[] = -{ +uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, [PX4IO_P_RAW_RC_FLAGS] = 0, [PX4IO_P_RAW_RC_NRSSI] = 0, [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_FRAME_COUNT] = 0, [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ...(PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; /** @@ -130,7 +130,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ...(PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 }; /** @@ -146,8 +146,7 @@ uint16_t r_page_scratch[32]; * * Setup registers */ -volatile uint16_t r_page_setup[] = -{ +volatile uint16_t r_page_setup[] = { #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 /* default to RSSI ADC functionality */ [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, @@ -171,30 +170,33 @@ volatile uint16_t r_page_setup[] = #endif [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, - [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, + [PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0, [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, [PX4IO_P_SETUP_PWM_REVERSE] = 0, + [PX4IO_P_SETUP_TRIM_ROLL] = 0, + [PX4IO_P_SETUP_TRIM_PITCH] = 0, + [PX4IO_P_SETUP_TRIM_YAW] = 0 }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ - PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ - PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI) + PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ + PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ + PX4IO_P_SETUP_FEATURES_PWM_RSSI) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ - PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ - PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ - 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_OVERRIDE_IMMEDIATE) + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ + PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ + 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_OVERRIDE_IMMEDIATE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -227,7 +229,7 @@ uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STR * PAGE 105 * * Failsafe servo PWM values - * + * * Disable pulses as default. */ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; @@ -262,7 +264,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num switch (page) { - /* handle bulk controls input */ + /* handle bulk controls input */ case PX4IO_PAGE_CONTROLS: /* copy channel data */ @@ -279,10 +281,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; - + break; - /* handle raw PWM input */ + /* handle raw PWM input */ case PX4IO_PAGE_DIRECT_PWM: /* copy channel data */ @@ -303,7 +305,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num break; - /* handle setup for servo failsafe values */ + /* handle setup for servo failsafe values */ case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ @@ -313,8 +315,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values < PWM_LOWEST_MIN) { r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; + } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; + } else { r_page_servo_failsafe[offset] = *values; } @@ -326,6 +330,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; case PX4IO_PAGE_CONTROL_MIN_PWM: @@ -337,8 +342,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values > PWM_HIGHEST_MIN) { r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; + } else if (*values < PWM_LOWEST_MIN) { r_page_servo_control_min[offset] = PWM_LOWEST_MIN; + } else { r_page_servo_control_min[offset] = *values; } @@ -347,8 +354,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; - + case PX4IO_PAGE_CONTROL_MAX_PWM: /* copy channel data */ @@ -358,8 +366,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; + } else if (*values < PWM_LOWEST_MAX) { r_page_servo_control_max[offset] = PWM_LOWEST_MAX; + } else { r_page_servo_control_max[offset] = *values; } @@ -368,10 +378,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; - case PX4IO_PAGE_DISARMED_PWM: - { + case PX4IO_PAGE_DISARMED_PWM: { /* flag for all outputs */ bool all_disarmed_off = true; @@ -381,12 +391,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* 0 means disabling always PWM */ r_page_servo_disarmed[offset] = 0; + } else if (*values < PWM_LOWEST_MIN) { r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; all_disarmed_off = false; + } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; all_disarmed_off = false; + } else { r_page_servo_disarmed[offset] = *values; all_disarmed_off = false; @@ -400,6 +413,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (all_disarmed_off) { /* disable PWM output if disarmed */ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); + } else { /* enable PWM output always */ r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; @@ -407,7 +421,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; - /* handle text going to the mixer parser */ + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: /* 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 @@ -416,19 +430,25 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num return mixer_handle_text(values, num_values * sizeof(*values)); default: + /* avoid offset wrap */ - if ((offset + num_values) > 255) + if ((offset + num_values) > 255) { num_values = 255 - offset; + } /* iterate individual registers, set each in turn */ while (num_values--) { - if (registers_set_one(page, offset, *values)) + if (registers_set_one(page, offset, *values)) { return -1; + } + offset++; values++; } + break; } + return 0; } @@ -445,19 +465,33 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_STATUS_FLAGS: - /* + + /* * Allow FMU override of arming state (to allow in-air restores), * but only if the arming state is not in sync on the IO side. */ - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + + if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + + } else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { r_status_flags = value; + } + + if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) { + + /* update failsafe values, now that the mixer is set to ok */ + mixer_set_failsafe(); + } + break; default: /* just ignore writes to other registers in this page */ break; } + break; case PX4IO_PAGE_SETUP: @@ -469,36 +503,37 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ /* switch S.Bus output pin as needed */ - #ifdef ENABLE_SBUS_OUT +#ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); /* disable the conflicting options with SBUS 1 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* disable the conflicting options with SBUS 2 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT); } - #endif + +#endif /* disable the conflicting options with ADC RSSI */ if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* apply changes */ @@ -548,9 +583,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (value < 25) { value = 25; } + if (value > 400) { value = 400; } + pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; @@ -558,13 +595,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (value < 25) { value = 25; } + if (value > 400) { value = 400; } + pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; @@ -595,10 +635,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; } - // we schedule a reboot rather than rebooting - // immediately to allow the IO board to ACK - // the reboot command - schedule_reboot(100000); + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); break; case PX4IO_P_SETUP_DSM: @@ -608,119 +648,134 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_FORCE_SAFETY_ON: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; - } else { - return -1; } + break; case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; - } else { - return -1; } + break; case PX4IO_P_SETUP_RC_THR_FAILSAFE_US: if (value > 650 && value < 2350) { r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value; } + break; case PX4IO_P_SETUP_PWM_REVERSE: r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] = value; break; + case PX4IO_P_SETUP_TRIM_ROLL: + case PX4IO_P_SETUP_TRIM_PITCH: + case PX4IO_P_SETUP_TRIM_YAW: + r_page_setup[offset] = value; + break; + default: return -1; } + break; case PX4IO_PAGE_RC_CONFIG: { - /** - * do not allow a RC config change while safety is off - */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { - break; - } + /** + * do not allow a RC config change while safety is off + */ + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { + break; + } - unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; - unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; - uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; + unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; + uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - if (channel >= PX4IO_RC_INPUT_CHANNELS) - return -1; + if (channel >= PX4IO_RC_INPUT_CHANNELS) { + return -1; + } - /* disable the channel until we have a chance to sanity-check it */ - conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + /* disable the channel until we have a chance to sanity-check it */ + conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - switch (index) { + switch (index) { - case PX4IO_P_RC_CONFIG_MIN: - case PX4IO_P_RC_CONFIG_CENTER: - case PX4IO_P_RC_CONFIG_MAX: - case PX4IO_P_RC_CONFIG_DEADZONE: - case PX4IO_P_RC_CONFIG_ASSIGNMENT: - conf[index] = value; - break; + case PX4IO_P_RC_CONFIG_MIN: + case PX4IO_P_RC_CONFIG_CENTER: + case PX4IO_P_RC_CONFIG_MAX: + case PX4IO_P_RC_CONFIG_DEADZONE: + case PX4IO_P_RC_CONFIG_ASSIGNMENT: + conf[index] = value; + break; - case PX4IO_P_RC_CONFIG_OPTIONS: - value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; - r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + case PX4IO_P_RC_CONFIG_OPTIONS: + value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; - /* clear any existing RC disabled flag */ - r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); + /* clear any existing RC disabled flag */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); - /* set all options except the enabled option */ - conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + /* set all options except the enabled option */ + conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - /* should the channel be enabled? */ - /* this option is normally set last */ - if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - uint8_t count = 0; - bool disabled = false; + /* should the channel be enabled? */ + /* this option is normally set last */ + if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint8_t count = 0; + bool disabled = false; - /* assert min..center..max ordering */ - if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { - count++; - } - /* assert deadzone is sane */ - if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { - count++; - } + /* assert min..center..max ordering */ + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { + count++; + } - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { - disabled = true; - } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && - (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { - count++; - } + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { + count++; + } + + /* assert deadzone is sane */ + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { + disabled = true; - /* sanity checks pass, enable channel */ - if (count) { - isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; - } else if (!disabled) { - conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && + (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { + count++; + } + + /* sanity checks pass, enable channel */ + if (count) { + isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; + + } else if (!disabled) { + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } } + + break; + /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ + } - break; - /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ + break; + /* case PX4IO_RC_PAGE_CONFIG */ } - break; - /* case PX4IO_RC_PAGE_CONFIG */ - } case PX4IO_PAGE_TEST: switch (offset) { @@ -728,11 +783,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) LED_AMBER(value & 1); break; } + break; default: return -1; } + return 0; } @@ -782,6 +839,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * */ unsigned counts = adc_measure(ADC_VBATT); + if (counts != 0xffff) { unsigned mV = (166460 + (counts * 45934)) / 10000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; @@ -801,6 +859,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val configuration for their sensor */ unsigned counts = adc_measure(ADC_IBATT); + if (counts != 0xffff) { r_page_status[PX4IO_P_STATUS_IBATT] = counts; } @@ -810,6 +869,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_VSERVO */ { unsigned counts = adc_measure(ADC_VSERVO); + if (counts != 0xffff) { // use 3:1 scaling on 3.3V ADC input unsigned mV = counts * 9900 / 4096; @@ -821,6 +881,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_VRSSI */ { unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { // use 1:1 scaling on 3.3V ADC input unsigned mV = counts * 3300 / 4096; @@ -853,8 +914,10 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_PWM_INFO: memset(r_page_scratch, 0, sizeof(r_page_scratch)); - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); + } SELECT_PAGE(r_page_scratch); break; @@ -867,15 +930,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_CONFIG: SELECT_PAGE(r_page_config); break; + case PX4IO_PAGE_ACTUATORS: SELECT_PAGE(r_page_actuators); break; + case PX4IO_PAGE_SERVOS: SELECT_PAGE(r_page_servos); break; + case PX4IO_PAGE_RAW_RC_INPUT: SELECT_PAGE(r_page_raw_rc_input); break; + case PX4IO_PAGE_RC_INPUT: SELECT_PAGE(r_page_rc_input); break; @@ -884,24 +951,31 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_SETUP: SELECT_PAGE(r_page_setup); break; + case PX4IO_PAGE_CONTROLS: SELECT_PAGE(r_page_controls); break; + case PX4IO_PAGE_RC_CONFIG: SELECT_PAGE(r_page_rc_input_config); break; + case PX4IO_PAGE_DIRECT_PWM: SELECT_PAGE(r_page_servos); break; + case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: SELECT_PAGE(r_page_servo_control_min); break; + case PX4IO_PAGE_CONTROL_MAX_PWM: SELECT_PAGE(r_page_servo_control_max); break; + case PX4IO_PAGE_DISARMED_PWM: SELECT_PAGE(r_page_servo_disarmed); break; @@ -913,12 +987,13 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val #undef SELECT_PAGE #undef COPY_PAGE -last_page = page; -last_offset = offset; + last_page = page; + last_offset = offset; /* if the offset is at or beyond the end of the page, we have no data */ - if (offset >= *num_values) + if (offset >= *num_values) { return -1; + } /* correct the data pointer and count for the offset */ *values += offset; @@ -938,8 +1013,10 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) /* get the channel mask for this rate group */ uint32_t mask = up_pwm_servo_get_rate_group(group); - if (mask == 0) + + if (mask == 0) { continue; + } /* all channels in the group must be either default or alt-rate */ uint32_t alt = map & mask; @@ -951,18 +1028,23 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; return; } + } else { /* set it - errors here are unexpected */ if (alt != 0) { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) { r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + } else { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) { r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } } } } } + r_setup_pwm_rates = map; r_setup_pwm_defaultrate = defaultrate; r_setup_pwm_altrate = altrate; diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 06f8733a528f..f4ab74a45a6c 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -110,7 +110,7 @@ safety_check_button(void *arg) * length in all cases of the if/else struct below. */ if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { + (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; @@ -149,6 +149,7 @@ safety_check_button(void *arg) } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_FMU_ARMED; + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) { pattern = LED_PATTERN_FMU_OK_TO_ARM; @@ -176,6 +177,7 @@ failsafe_blink(void *arg) /* blink the failsafe LED if we don't have FMU input */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { failsafe = !failsafe; + } else { failsafe = false; } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index e9adc8cd6970..300ec0b78400 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -131,14 +131,19 @@ interface_init(void) rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; #if 0 /* keep this for signal integrity testing */ + for (;;) { while (!(rSR & USART_SR_TXE)) ; + rDR = 0xfa; + while (!(rSR & USART_SR_TXE)) ; + rDR = 0xa0; } + #endif /* configure RX DMA and return to listening state */ @@ -153,6 +158,7 @@ rx_handle_packet(void) /* check packet CRC */ uint8_t crc = dma_packet.crc; dma_packet.crc = 0; + if (crc != crc_packet(&dma_packet)) { perf_count(pc_crcerr); @@ -170,11 +176,13 @@ rx_handle_packet(void) if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { perf_count(pc_regerr); dma_packet.count_code = PKT_CODE_ERROR; + } else { dma_packet.count_code = PKT_CODE_SUCCESS; } + return; - } + } if (PKT_CODE(dma_packet) == PKT_CODE_READ) { @@ -185,17 +193,22 @@ rx_handle_packet(void) if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { perf_count(pc_regerr); dma_packet.count_code = PKT_CODE_ERROR; + } else { /* constrain reply to requested size */ - if (count > PKT_MAX_REGS) + if (count > PKT_MAX_REGS) { count = PKT_MAX_REGS; - if (count > PKT_COUNT(dma_packet)) + } + + if (count > PKT_COUNT(dma_packet)) { count = PKT_COUNT(dma_packet); + } /* copy reply registers into DMA buffer */ memcpy((void *)&dma_packet.regs[0], registers, count * 2); dma_packet.count_code = count | PKT_CODE_SUCCESS; } + return; } @@ -250,16 +263,22 @@ serial_interrupt(int irq, void *context) (void)rDR; /* required to clear any of the interrupt status that brought us here */ if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ - USART_SR_NE | /* noise error - we have lost a byte due to noise */ - USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ perf_count(pc_errors); - if (sr & USART_SR_ORE) + + if (sr & USART_SR_ORE) { perf_count(pc_ore); - if (sr & USART_SR_NE) + } + + if (sr & USART_SR_NE) { perf_count(pc_ne); - if (sr & USART_SR_FE) + } + + if (sr & USART_SR_FE) { perf_count(pc_fe); + } /* send a line break - this will abort transmission/reception on the other end */ rCR1 |= USART_CR1_SBK; @@ -270,7 +289,7 @@ serial_interrupt(int irq, void *context) if (sr & USART_SR_IDLE) { - /* + /* * If we saw an error, don't bother looking at the packet - it should have * been aborted by the sender and will definitely be bad. Get the DMA reconfigured * ready for their retry. @@ -284,10 +303,11 @@ serial_interrupt(int irq, void *context) /* * The sender has stopped sending - this is probably the end of a packet. - * Check the received length against the length in the header to see if + * Check the received length against the length in the header to see if * we have something that looks like a packet. */ unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); + if ((length < 1) || (length < PKT_SIZE(dma_packet))) { /* it was too short - possibly truncated */ From 6d7e29c0277d064bfd8b1e4a02069cfa2be910cd Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 6 Apr 2015 18:22:32 -0700 Subject: [PATCH 232/293] px4io: return failure when FORCE_SAFETY_OFF fails --- src/modules/px4iofirmware/registers.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a3351bcbd5bd..99435ca10429 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -648,6 +648,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_FORCE_SAFETY_ON: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } else { + return -1; } break; @@ -655,6 +657,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } else { + return -1; } break; From 5fc84fec1d757dd8ff14bbcc9edd85abc99a2d58 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 11:00:33 +1100 Subject: [PATCH 233/293] rc: fixed build for FMUv2 and FMUv1 --- src/lib/rc/dsm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index c068cdae0ca9..e1bd7cd09583 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -50,11 +50,11 @@ #include #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -#include +#include #endif #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 -#include +#include #endif #define DSM_FRAME_SIZE 16 /** Date: Sun, 21 Jun 2015 16:29:07 +1000 Subject: [PATCH 234/293] px4iofirmware: blink blue LED more rapidly when override is active this makes it easier for a user to test that override is working correctly in pre-flight checks. Otherwise override is hard to distinguish from normal manual mode --- src/modules/px4iofirmware/px4io.c | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 1ee50d5a7f64..9dc24f5c26fe 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -363,10 +363,21 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); - if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) { - last_heartbeat_time = hrt_absolute_time(); - heartbeat_blink(); - } + /* + blink blue LED at 4Hz in normal operation. When in + override blink 4x faster so the user can clearly see + that override is happening. This helps when + pre-flight testing the override system + */ + uint32_t heartbeat_period_us = 250*1000UL; + if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { + heartbeat_period_us /= 4; + } + + if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } ring_blink(); From 12a1ebcbf306985bcfbc8c32f0f03b378c1c8e4b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 21 Jun 2015 18:02:31 +1000 Subject: [PATCH 235/293] px4iofirmware: allow override when RAW_PWM is active if override is enabled then it should apply even if RAW_PWM is being supplied by the FMU --- src/modules/px4iofirmware/mixer.cpp | 35 ++++++++++++++++------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 4ff95bd5afe3..64abd2b7f123 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -121,13 +121,23 @@ mixer_tick(void) * Decide which set of controls we're using. */ + bool override_enabled = ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)); + /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - /* don't actually mix anything - we already have raw PWM values */ - source = MIX_NONE; - + if (override_enabled) { + /* a channel based override has been + * triggered, with FMU active */ + source = MIX_OVERRIDE_FMU_OK; + } else { + /* don't actually mix anything - we already have raw PWM values */ + source = MIX_NONE; + } } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && @@ -138,22 +148,15 @@ mixer_tick(void) source = MIX_FMU; } - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - /* do not enter manual override if we asked for termination failsafe and FMU is lost */ - !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { + if ( override_enabled && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + /* do not enter manual override if we asked for termination failsafe and FMU is lost */ + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; - - } else if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + } else if ( override_enabled && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly up to available rc channels */ source = MIX_OVERRIDE_FMU_OK; From 70e2344be03eba882303472e9d2bc1426ea48692 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 11:32:49 +1100 Subject: [PATCH 236/293] px4io: update for new debug macros --- src/drivers/px4io/px4io.cpp | 72 ++++++++++++++++++------------------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d7e1b56fb45c..d1c3f1c6c0cd 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -592,10 +592,10 @@ PX4IO::detect() if (protocol != PX4IO_PROTOCOL_VERSION) { if (protocol == _io_reg_get_error) { - log("IO not installed"); + DEVICE_LOG("IO not installed"); } else { - log("IO version error"); + DEVICE_LOG("IO version error"); mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); } @@ -603,7 +603,7 @@ PX4IO::detect() } } - log("IO found"); + DEVICE_LOG("IO found"); return 0; } @@ -667,13 +667,13 @@ PX4IO::init() (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { - log("config read error"); + DEVICE_LOG("config read error"); mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort."); return -1; } - if (_max_rc_input > RC_INPUT_MAX_CHANNELS) - _max_rc_input = RC_INPUT_MAX_CHANNELS; + if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) + _max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS; if (!_rc_handling_disabled) { param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); @@ -798,7 +798,7 @@ PX4IO::init() /* re-send if necessary */ if (!safety.armed) { orb_publish(ORB_ID(vehicle_command), pub, &cmd); - log("re-sending arm cmd"); + DEVICE_LOG("re-sending arm cmd"); } /* keep waiting for state change for 2 s */ @@ -824,7 +824,7 @@ PX4IO::init() ret = io_disable_rc_handling(); if (ret != OK) { - log("failed disabling RC handling"); + DEVICE_LOG("failed disabling RC handling"); return ret; } @@ -853,7 +853,7 @@ PX4IO::init() ret = register_driver(PWM_OUTPUT0_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { - log("default PWM output device"); + DEVICE_LOG("default PWM output device"); _primary_pwm_device = true; } @@ -866,7 +866,7 @@ PX4IO::init() nullptr); if (_task < 0) { - debug("task start failed: %d", errno); + DEVICE_DEBUG("task start failed: %d", errno); return -errno; } @@ -1128,7 +1128,7 @@ PX4IO::task_main() unlock(); out: - debug("exiting"); + DEVICE_DEBUG("exiting"); /* clean up the alternate device node */ if (_primary_pwm_device) @@ -1439,7 +1439,7 @@ PX4IO::io_set_rc_config() ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); if (ret != OK) { - log("rc config upload failed"); + DEVICE_LOG("rc config upload failed"); break; } @@ -1641,10 +1641,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) int ret; /* we don't have the status bits, so input_source has to be set elsewhere */ - input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); - uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; + uint16_t regs[input_rc_s::RC_INPUT_MAX_CHANNELS + prolog]; /* * Read the channel count and the first 9 channels. @@ -1663,8 +1663,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) channel_count = regs[PX4IO_P_RAW_RC_COUNT]; /* limit the channel count */ - if (channel_count > RC_INPUT_MAX_CHANNELS) { - channel_count = RC_INPUT_MAX_CHANNELS; + if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; } _rc_chan_count = channel_count; @@ -1702,7 +1702,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) } /* get RSSI from input channel */ - if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { + if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) / ((_rssi_pwm_max - _rssi_pwm_min) / 100); rssi = rssi > 100 ? 100 : rssi; @@ -1730,19 +1730,19 @@ PX4IO::io_publish_raw_rc() /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_ST24; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24; } else { - rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; /* only keep publishing RC input if we ever got a valid input */ if (_rc_last_valid == 0) { @@ -1817,14 +1817,14 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned { /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { - debug("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2); + DEVICE_DEBUG("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2); return -EINVAL; } int ret = _interface->write((page << 8) | offset, (void *)values, num_values); if (ret != (int)num_values) { - debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); + DEVICE_DEBUG("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); return -1; } @@ -1842,14 +1842,14 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v { /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { - debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); + DEVICE_DEBUG("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); return -EINVAL; } int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); if (ret != (int)num_values) { - debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); + DEVICE_DEBUG("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); return -1; } @@ -1996,7 +1996,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } if (ret) { - log("mixer send error %d", ret); + DEVICE_LOG("mixer send error %d", ret); return ret; } @@ -2026,7 +2026,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) retries--; - log("mixer sent"); + DEVICE_LOG("mixer sent"); } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK))); @@ -2036,7 +2036,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) return 0; } - log("mixer rejected by IO"); + DEVICE_LOG("mixer rejected by IO"); mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); /* load must have failed for some reason */ @@ -2619,19 +2619,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) /* sort out the source of the values */ if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM; } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; } else if (status & PX4IO_P_STATUS_FLAGS_RC_ST24) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_ST24; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24; } else { - rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; } /* read raw R/C input values */ @@ -2661,7 +2661,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) if (ret != OK) return ret; if (io_crc != arg) { - debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); + DEVICE_DEBUG("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); return -EINVAL; } break; @@ -2716,7 +2716,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 >= RC_INPUT_MAX_CHANNELS) { + if (config->channel >= input_rc_s::RC_INPUT_MAX_CHANNELS) { /* fail with error */ return -E2BIG; } From 3e7a1201edcd449efabfae4dec4c6c87d355ce31 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 11:32:58 +1100 Subject: [PATCH 237/293] oreoled: update for new debug macros --- src/drivers/oreoled/oreoled.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index b6095aa6b9ed..db7038352915 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -258,10 +258,10 @@ OREOLED::info() /* print health info on each LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (!_healthy[i]) { - log("oreo %u: BAD", (unsigned)i); + DEVICE_LOG("oreo %u: BAD", (unsigned)i); } else { - log("oreo %u: OK", (unsigned)i); + DEVICE_LOG("oreo %u: OK", (unsigned)i); } } @@ -334,7 +334,7 @@ OREOLED::cycle() if (transfer(msg, sizeof(msg), reply, 3) == OK) { if (reply[1] == OREOLED_BASE_I2C_ADDR + i && reply[2] == msg[sizeof(msg) - 1]) { - log("oreoled %u ok - in bootloader", (unsigned)i); + DEVICE_LOG("oreoled %u ok - in bootloader", (unsigned)i); _healthy[i] = true; _num_healthy++; @@ -350,12 +350,12 @@ OREOLED::cycle() which indicates a response from firmwares >= 1.3 */ } else if (reply[1] == OREOLED_BASE_I2C_ADDR + i && reply[2] == msg[sizeof(msg) - 1] + 1) { - log("oreoled %u ok - in application", (unsigned)i); + DEVICE_LOG("oreoled %u ok - in application", (unsigned)i); _healthy[i] = true; _num_healthy++; } else { - log("oreo reply errors: %u", (unsigned)_reply_errors); + DEVICE_LOG("oreo reply errors: %u", (unsigned)_reply_errors); perf_count(_reply_errors); } From 706217c7f72cecd07b315df4584de7fcc7ff30bd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 11:33:06 +1100 Subject: [PATCH 238/293] l3gd20: update for new debug macros --- src/drivers/l3gd20/l3gd20.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 66ef6c9b07ef..3c9194243dde 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -493,7 +493,7 @@ L3GD20::init() &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { - debug("failed to create sensor_gyro publication"); + DEVICE_DEBUG("failed to create sensor_gyro publication"); } ret = OK; @@ -910,7 +910,7 @@ L3GD20::disable_i2c(void) return; } } - debug("FAILED TO DISABLE I2C"); + DEVICE_DEBUG("FAILED TO DISABLE I2C"); } void From eccbbb654580fab917a3c68509f2850654b0bf37 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 11:33:17 +1100 Subject: [PATCH 239/293] px4io-v2: fixed paths --- src/drivers/boards/px4io-v2/module.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk index 3f0e9a0b3a22..f3111f6bc267 100644 --- a/src/drivers/boards/px4io-v2/module.mk +++ b/src/drivers/boards/px4io-v2/module.mk @@ -2,7 +2,7 @@ # Board-specific startup code for the PX4IOv2 # -SRCS = px4iov2_init.c \ - px4iov2_pwm_servo.c +SRCS = px4io_init.c \ + px4io_pwm_servo.c MAXOPTIMIZATION = -Os From 15604892409323da7bc0ce17539521f50d178977 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 11:34:14 +1100 Subject: [PATCH 240/293] modules: update from upstream --- .../attitude_estimator_ekf/CMakeLists.txt | 45 + .../attitude_estimator_ekf_main.cpp | 78 +- .../attitude_estimator_ekf_params.c | 53 - .../attitude_estimator_q/CMakeLists.txt | 47 + .../attitude_estimator_q_main.cpp | 445 +++- .../attitude_estimator_q_params.c | 35 + src/modules/attitude_estimator_so3/README | 3 - .../attitude_estimator_so3_main.cpp | 661 ------ .../attitude_estimator_so3_params.c | 86 - src/modules/attitude_estimator_so3/module.mk | 12 - src/modules/bottle_drop/CMakeLists.txt | 44 + src/modules/bottle_drop/bottle_drop.cpp | 143 +- src/modules/commander/CMakeLists.txt | 60 + src/modules/commander/PreflightCheck.cpp | 255 ++- src/modules/commander/PreflightCheck.h | 2 +- .../commander/accelerometer_calibration.cpp | 28 +- .../commander/airspeed_calibration.cpp | 52 +- src/modules/commander/calibration_messages.h | 2 +- .../commander/calibration_routines.cpp | 12 +- src/modules/commander/commander.cpp | 868 ++++++-- src/modules/commander/commander_helper.cpp | 49 +- src/modules/commander/commander_helper.h | 3 + src/modules/commander/commander_params.c | 110 +- .../commander/commander_tests/CMakeLists.txt | 44 + src/modules/commander/gyro_calibration.cpp | 37 +- src/modules/commander/mag_calibration.cpp | 163 +- src/modules/commander/px4_custom_mode.h | 42 +- src/modules/commander/rc_calibration.cpp | 34 +- .../commander/state_machine_helper.cpp | 308 +-- src/modules/commander/state_machine_helper.h | 2 +- .../commander/state_machine_helper_posix.cpp | 634 ------ src/modules/controllib/CMakeLists.txt | 48 + src/modules/controllib/block/Block.hpp | 33 +- src/modules/controllib/block/BlockParam.cpp | 20 +- src/modules/controllib/block/BlockParam.hpp | 1 + src/modules/controllib/blocks.cpp | 271 ++- src/modules/controllib/blocks.hpp | 63 +- .../controllib/controllib_test_main.cpp | 51 + src/modules/controllib/uorb/blocks.cpp | 35 +- src/modules/controllib/uorb/blocks.hpp | 8 +- src/modules/dataman/CMakeLists.txt | 44 + src/modules/dataman/dataman.c | 236 ++- src/modules/dataman/dataman.h | 173 +- .../AttitudePositionEstimatorEKF.h | 61 +- .../ekf_att_pos_estimator/CMakeLists.txt | 51 + .../ekf_att_pos_estimator_main.cpp | 950 +++++---- .../ekf_att_pos_estimator_params.c | 70 +- .../estimator_22states.cpp | 44 +- .../estimator_22states.h | 3 + .../estimator_utilities.cpp | 17 +- src/modules/fixedwing_backside/CMakeLists.txt | 43 + src/modules/fixedwing_backside/fixedwing.cpp | 145 +- .../fixedwing_backside_main.cpp | 13 +- src/modules/fw_att_control/CMakeLists.txt | 44 + .../fw_att_control/fw_att_control_main.cpp | 469 +++-- .../fw_att_control/fw_att_control_params.c | 122 +- src/modules/fw_pos_control_l1/CMakeLists.txt | 50 + .../fw_pos_control_l1_main.cpp | 1010 ++++++--- .../fw_pos_control_l1_params.c | 38 +- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 78 +- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 26 +- .../fw_pos_control_l1/mtecs/mTecs_params.c | 3 - src/modules/gpio_led/CMakeLists.txt | 43 + src/modules/gpio_led/gpio_led.c | 2 + src/modules/land_detector/CMakeLists.txt | 47 + .../land_detector/FixedwingLandDetector.cpp | 52 +- .../land_detector/FixedwingLandDetector.h | 16 +- src/modules/land_detector/LandDetector.cpp | 81 +- src/modules/land_detector/LandDetector.h | 17 +- .../land_detector/MulticopterLandDetector.cpp | 48 +- .../land_detector/MulticopterLandDetector.h | 14 +- .../land_detector/land_detector_main.cpp | 72 +- .../land_detector/land_detector_params.c | 49 +- .../BlockLocalPositionEstimator.cpp | 1378 +++++++++++++ .../BlockLocalPositionEstimator.hpp | 310 +++ .../local_position_estimator/CMakeLists.txt | 55 + .../local_position_estimator_main.cpp | 169 ++ src/modules/local_position_estimator/params.c | 222 ++ src/modules/mavlink/CMakeLists.txt | 62 + src/modules/mavlink/mavlink.c | 58 +- src/modules/mavlink/mavlink_ftp.cpp | 10 +- src/modules/mavlink/mavlink_ftp.h | 2 +- src/modules/mavlink/mavlink_main.cpp | 495 ++++- src/modules/mavlink/mavlink_main.h | 71 +- src/modules/mavlink/mavlink_main_posix.cpp | 1818 ----------------- src/modules/mavlink/mavlink_messages.cpp | 421 +++- src/modules/mavlink/mavlink_mission.cpp | 122 +- src/modules/mavlink/mavlink_mission.h | 23 +- src/modules/mavlink/mavlink_parameters.cpp | 214 +- src/modules/mavlink/mavlink_parameters.h | 8 +- src/modules/mavlink/mavlink_params.c | 102 + src/modules/mavlink/mavlink_receiver.cpp | 395 +++- src/modules/mavlink/mavlink_receiver.h | 42 +- .../mavlink/mavlink_tests/CMakeLists.txt | 53 + src/modules/mc_att_control/CMakeLists.txt | 46 + .../mc_att_control/mc_att_control_main.cpp | 113 +- .../mc_att_control/mc_att_control_params.c | 58 +- .../CMakeLists.txt | 44 + .../mc_att_control.cpp | 16 +- .../mc_att_control.h | 8 +- .../mc_att_control_main.cpp | 13 +- .../mc_att_control_params.c | 6 +- .../mc_att_control_params.h | 2 +- .../mc_att_control_start_nuttx.cpp | 33 +- src/modules/mc_pos_control/CMakeLists.txt | 42 + .../mc_pos_control/mc_pos_control_main.cpp | 476 +++-- .../mc_pos_control/mc_pos_control_params.c | 104 +- .../CMakeLists.txt | 44 + .../mc_pos_control.cpp | 51 +- .../mc_pos_control.h | 6 +- .../mc_pos_control_main.cpp | 13 +- .../mc_pos_control_start_nuttx.cpp | 30 +- src/modules/muorb/adsp/CMakeLists.txt | 45 + src/modules/muorb/adsp/module.mk | 50 + src/modules/muorb/adsp/px4muorb.cpp | 163 ++ src/modules/muorb/adsp/px4muorb.hpp | 58 + src/modules/muorb/adsp/uORBFastRpcChannel.cpp | 553 +++++ src/modules/muorb/adsp/uORBFastRpcChannel.hpp | 267 +++ .../krait-stubs/px4muorb_KraitRpcWrapper.hpp | 76 + src/modules/muorb/krait/CMakeLists.txt | 46 + src/modules/muorb/krait/module.mk | 48 + src/modules/muorb/krait/muorb_main.cpp | 84 + .../muorb/krait/uORBKraitFastRpcChannel.cpp | 345 ++++ .../muorb/krait/uORBKraitFastRpcChannel.hpp | 154 ++ src/modules/navigator/CMakeLists.txt | 56 + src/modules/navigator/datalinkloss.cpp | 1 + src/modules/navigator/datalinkloss_params.c | 19 +- src/modules/navigator/enginefailure.cpp | 1 + src/modules/navigator/geofence.cpp | 34 +- src/modules/navigator/geofence.h | 17 +- src/modules/navigator/geofence_params.c | 16 +- src/modules/navigator/gpsfailure.cpp | 1 + src/modules/navigator/gpsfailure_params.c | 4 - src/modules/navigator/loiter.cpp | 5 +- src/modules/navigator/loiter.h | 3 + src/modules/navigator/mission.cpp | 231 ++- src/modules/navigator/mission.h | 12 +- src/modules/navigator/mission_block.cpp | 42 +- src/modules/navigator/mission_block.h | 9 +- .../navigator/mission_feasibility_checker.cpp | 167 +- .../navigator/mission_feasibility_checker.h | 13 +- src/modules/navigator/mission_params.c | 10 +- src/modules/navigator/navigation.h | 115 ++ src/modules/navigator/navigator.h | 26 +- src/modules/navigator/navigator_main.cpp | 102 +- src/modules/navigator/navigator_params.c | 30 +- src/modules/navigator/rcloss.cpp | 2 +- src/modules/navigator/rcloss_params.c | 4 - src/modules/navigator/rtl.cpp | 19 +- src/modules/navigator/rtl.h | 5 +- src/modules/navigator/rtl_params.c | 4 - src/modules/param/CMakeLists.txt | 49 + .../position_estimator_inav/CMakeLists.txt | 48 + .../position_estimator_inav_main.c | 362 +++- .../position_estimator_inav_params.c | 93 +- .../position_estimator_inav_params.h | 24 +- src/modules/sdlog2/CMakeLists.txt | 50 + src/modules/sdlog2/logbuffer.c | 14 +- src/modules/sdlog2/params.c | 75 + src/modules/sdlog2/sdlog2.c | 535 +++-- src/modules/sdlog2/sdlog2_messages.h | 90 +- src/modules/segway/BlockSegwayController.cpp | 12 +- src/modules/segway/BlockSegwayController.hpp | 5 +- src/modules/segway/CMakeLists.txt | 43 + src/modules/segway/segway_main.cpp | 13 +- src/modules/sensors/CMakeLists.txt | 46 + src/modules/sensors/sensor_params.c | 1333 ++++++++++-- src/modules/sensors/sensors.cpp | 728 +++---- src/modules/simulator/CMakeLists.txt | 54 + src/modules/simulator/simulator.cpp | 246 +-- src/modules/simulator/simulator.h | 195 +- src/modules/simulator/simulator_mavlink.cpp | 732 +++++-- .../mixer/mixer_multirotor.generated.h | 168 ++ src/modules/uORB/CMakeLists.txt | 83 + src/modules/uORB/ORBMap.hpp | 146 ++ src/modules/uORB/ORBSet.hpp | 146 ++ src/modules/uORB/Publication.cpp | 63 +- src/modules/uORB/Publication.hpp | 81 +- src/modules/uORB/Subscription.cpp | 93 +- src/modules/uORB/Subscription.hpp | 76 +- src/modules/uORB/objects_common.cpp | 46 +- src/modules/uORB/uORB.cpp | 43 +- src/modules/uORB/uORB.h | 29 +- src/modules/uORB/uORBCommon.hpp | 30 +- src/modules/uORB/uORBCommunicator.hpp | 179 ++ src/modules/uORB/uORBDevices_nuttx.cpp | 892 ++++---- src/modules/uORB/uORBDevices_nuttx.hpp | 288 +-- src/modules/uORB/uORBDevices_posix.cpp | 939 +++++---- src/modules/uORB/uORBDevices_posix.hpp | 168 +- src/modules/uORB/uORBMain.cpp | 152 +- src/modules/uORB/uORBManager.hpp | 586 +++--- src/modules/uORB/uORBManager_nuttx.cpp | 437 ++-- src/modules/uORB/uORBManager_posix.cpp | 463 +++-- src/modules/uORB/uORBTest_UnitTest.cpp | 509 +++-- src/modules/uORB/uORBTest_UnitTest.hpp | 141 +- src/modules/uORB/uORBUtils.cpp | 51 +- src/modules/uORB/uORBUtils.hpp | 23 +- src/modules/unit_test/CMakeLists.txt | 42 + src/modules/unit_test/unit_test.h | 23 +- src/modules/vtol_att_control/CMakeLists.txt | 48 + src/modules/vtol_att_control/standard.cpp | 325 +++ src/modules/vtol_att_control/standard.h | 104 + .../standard_params.c} | 38 +- src/modules/vtol_att_control/tailsitter.cpp | 199 ++ src/modules/vtol_att_control/tailsitter.h | 71 + src/modules/vtol_att_control/tiltrotor.cpp | 453 ++++ src/modules/vtol_att_control/tiltrotor.h | 162 ++ .../vtol_att_control/tiltrotor_params.c | 96 + .../vtol_att_control_main.cpp | 571 ++---- .../vtol_att_control/vtol_att_control_main.h | 220 ++ .../vtol_att_control_params.c | 91 +- src/modules/vtol_att_control/vtol_type.cpp | 133 ++ src/modules/vtol_att_control/vtol_type.h | 121 ++ 213 files changed, 22026 insertions(+), 10240 deletions(-) create mode 100644 src/modules/attitude_estimator_ekf/CMakeLists.txt create mode 100644 src/modules/attitude_estimator_q/CMakeLists.txt delete mode 100644 src/modules/attitude_estimator_so3/README delete mode 100755 src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp delete mode 100755 src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c delete mode 100644 src/modules/attitude_estimator_so3/module.mk create mode 100644 src/modules/bottle_drop/CMakeLists.txt create mode 100644 src/modules/commander/CMakeLists.txt create mode 100644 src/modules/commander/commander_tests/CMakeLists.txt delete mode 100644 src/modules/commander/state_machine_helper_posix.cpp create mode 100644 src/modules/controllib/CMakeLists.txt create mode 100644 src/modules/controllib/controllib_test_main.cpp create mode 100644 src/modules/dataman/CMakeLists.txt create mode 100644 src/modules/ekf_att_pos_estimator/CMakeLists.txt create mode 100644 src/modules/fixedwing_backside/CMakeLists.txt create mode 100644 src/modules/fw_att_control/CMakeLists.txt create mode 100644 src/modules/fw_pos_control_l1/CMakeLists.txt create mode 100644 src/modules/gpio_led/CMakeLists.txt create mode 100644 src/modules/land_detector/CMakeLists.txt create mode 100644 src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp create mode 100644 src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp create mode 100644 src/modules/local_position_estimator/CMakeLists.txt create mode 100644 src/modules/local_position_estimator/local_position_estimator_main.cpp create mode 100644 src/modules/local_position_estimator/params.c create mode 100644 src/modules/mavlink/CMakeLists.txt delete mode 100644 src/modules/mavlink/mavlink_main_posix.cpp create mode 100644 src/modules/mavlink/mavlink_params.c create mode 100644 src/modules/mavlink/mavlink_tests/CMakeLists.txt create mode 100644 src/modules/mc_att_control/CMakeLists.txt create mode 100644 src/modules/mc_att_control_multiplatform/CMakeLists.txt create mode 100644 src/modules/mc_pos_control/CMakeLists.txt create mode 100644 src/modules/mc_pos_control_multiplatform/CMakeLists.txt create mode 100644 src/modules/muorb/adsp/CMakeLists.txt create mode 100644 src/modules/muorb/adsp/module.mk create mode 100644 src/modules/muorb/adsp/px4muorb.cpp create mode 100644 src/modules/muorb/adsp/px4muorb.hpp create mode 100644 src/modules/muorb/adsp/uORBFastRpcChannel.cpp create mode 100644 src/modules/muorb/adsp/uORBFastRpcChannel.hpp create mode 100644 src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp create mode 100644 src/modules/muorb/krait/CMakeLists.txt create mode 100644 src/modules/muorb/krait/module.mk create mode 100644 src/modules/muorb/krait/muorb_main.cpp create mode 100644 src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp create mode 100644 src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp create mode 100644 src/modules/navigator/CMakeLists.txt create mode 100644 src/modules/navigator/navigation.h create mode 100644 src/modules/param/CMakeLists.txt create mode 100644 src/modules/position_estimator_inav/CMakeLists.txt create mode 100644 src/modules/sdlog2/CMakeLists.txt create mode 100644 src/modules/sdlog2/params.c create mode 100644 src/modules/segway/CMakeLists.txt create mode 100644 src/modules/sensors/CMakeLists.txt create mode 100644 src/modules/simulator/CMakeLists.txt create mode 100644 src/modules/systemlib/mixer/mixer_multirotor.generated.h create mode 100644 src/modules/uORB/CMakeLists.txt create mode 100644 src/modules/uORB/ORBMap.hpp create mode 100644 src/modules/uORB/ORBSet.hpp create mode 100644 src/modules/uORB/uORBCommunicator.hpp create mode 100644 src/modules/unit_test/CMakeLists.txt create mode 100644 src/modules/vtol_att_control/CMakeLists.txt create mode 100644 src/modules/vtol_att_control/standard.cpp create mode 100644 src/modules/vtol_att_control/standard.h rename src/modules/{attitude_estimator_so3/attitude_estimator_so3_params.h => vtol_att_control/standard_params.c} (68%) mode change 100755 => 100644 create mode 100644 src/modules/vtol_att_control/tailsitter.cpp create mode 100644 src/modules/vtol_att_control/tailsitter.h create mode 100644 src/modules/vtol_att_control/tiltrotor.cpp create mode 100644 src/modules/vtol_att_control/tiltrotor.h create mode 100644 src/modules/vtol_att_control/tiltrotor_params.c create mode 100644 src/modules/vtol_att_control/vtol_att_control_main.h create mode 100644 src/modules/vtol_att_control/vtol_type.cpp create mode 100644 src/modules/vtol_att_control/vtol_type.h diff --git a/src/modules/attitude_estimator_ekf/CMakeLists.txt b/src/modules/attitude_estimator_ekf/CMakeLists.txt new file mode 100644 index 000000000000..8c689046610d --- /dev/null +++ b/src/modules/attitude_estimator_ekf/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__attitude_estimator_ekf + MAIN attitude_estimator_ekf + STACK 1200 + COMPILE_FLAGS + -Wno-float-equal + SRCS + attitude_estimator_ekf_main.cpp + codegen/AttitudeEKF.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : 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 afd8706dbf50..e408fb3c9926 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include @@ -107,6 +108,46 @@ usage(const char *reason) fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); } +int parameters_init(struct attitude_estimator_ekf_param_handles *h) +{ + /* PID parameters */ + h->q0 = param_find("EKF_ATT_V3_Q0"); + 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->r0 = param_find("EKF_ATT_V4_R0"); + h->r1 = param_find("EKF_ATT_V4_R1"); + h->r2 = param_find("EKF_ATT_V4_R2"); + + 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; +} + +int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) +{ + param_get(h->q0, &(p->q[0])); + param_get(h->q1, &(p->q[1])); + param_get(h->q2, &(p->q[2])); + param_get(h->q3, &(p->q[3])); + + param_get(h->r0, &(p->r[0])); + param_get(h->r1, &(p->r[1])); + param_get(h->r2, &(p->r[2])); + + 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; +} + /** * The attitude_estimator_ekf app only briefly exists to start * the background job. The stack size assigned in the @@ -261,6 +302,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* subscribe to vision estimate */ int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); + /* subscribe to mocap data */ + int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); + /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -291,6 +335,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) R_decl.identity(); struct vision_position_estimate_s vision {}; + struct att_pos_mocap_s mocap {}; /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); @@ -378,10 +423,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_timestamp[0] != raw.timestamp) { + if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; + sensor_last_timestamp[0] = raw.gyro_timestamp[0]; } z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; @@ -389,10 +434,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.accelerometer_timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp[0]; } hrt_abstime vel_t = 0; @@ -432,24 +477,43 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ - if (sensor_last_timestamp[2] != raw.magnetometer_timestamp && + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] && /* check that the mag vector is > 0 */ fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.magnetometer_timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp[0]; } bool vision_updated = false; orb_check(vision_sub, &vision_updated); + bool mocap_updated = false; + orb_check(mocap_sub, &mocap_updated); + if (vision_updated) { orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision); } - if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { + if (mocap_updated) { + orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap); + } + + if (mocap.timestamp_boot > 0 && (hrt_elapsed_time(&mocap.timestamp_boot) < 500000)) { + + math::Quaternion q(mocap.q); + math::Matrix<3, 3> Rmoc = q.to_dcm(); + + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency + z_k[6] = vn(0); + z_k[7] = vn(1); + z_k[8] = vn(2); + }else if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { math::Quaternion q(vision.q); math::Matrix<3, 3> Rvis = q.to_dcm(); 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 30eb2a96e03b..92156c93e8a0 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -95,19 +95,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); -/** - * EKF attitude estimator enabled - * - * If enabled, it uses the older EKF filter. - * However users can enable the new quaternion - * based complimentary filter by setting EKF_ATT_ENABLED = 0. - * - * @min 0 - * @max 1 - * @group Attitude EKF estimator - */ -PARAM_DEFINE_INT32(EKF_ATT_ENABLED, 1); - /** * Moment of inertia matrix diagonal entry (1, 1) * @@ -142,43 +129,3 @@ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); * @max 1 */ PARAM_DEFINE_INT32(ATT_J_EN, 0); - -int parameters_init(struct attitude_estimator_ekf_param_handles *h) -{ - /* PID parameters */ - h->q0 = param_find("EKF_ATT_V3_Q0"); - 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->r0 = param_find("EKF_ATT_V4_R0"); - h->r1 = param_find("EKF_ATT_V4_R1"); - h->r2 = param_find("EKF_ATT_V4_R2"); - - 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; -} - -int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) -{ - param_get(h->q0, &(p->q[0])); - param_get(h->q1, &(p->q[1])); - param_get(h->q2, &(p->q[2])); - param_get(h->q3, &(p->q[3])); - - param_get(h->r0, &(p->r[0])); - param_get(h->r1, &(p->r[1])); - param_get(h->r2, &(p->r[2])); - - 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_q/CMakeLists.txt b/src/modules/attitude_estimator_q/CMakeLists.txt new file mode 100644 index 000000000000..c74993ad6742 --- /dev/null +++ b/src/modules/attitude_estimator_q/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################# +set(MODULE_CFLAGS) +if (${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=1400) +endif() +px4_add_module( + MODULE modules__attitude_estimator_q + MAIN attitude_estimator_q + COMPILE_FLAGS ${MODULE_CFLAGS} + STACK 1200 + SRCS + attitude_estimator_q_main.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index f999a8f51bce..4b4fba54c870 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -39,7 +39,8 @@ * @author Anton Babushkin */ -#include +#include +#include #include #include #include @@ -47,23 +48,25 @@ #include #include #include -#include -#include -#include #include #include #include #include -#include #include #include +#include #include #include +#include +#include #include #include -#include +#include +#include #include +#include +#include #include #include @@ -78,12 +81,14 @@ using math::Quaternion; class AttitudeEstimatorQ; -namespace attitude_estimator_q { +namespace attitude_estimator_q +{ AttitudeEstimatorQ *instance; } -class AttitudeEstimatorQ { +class AttitudeEstimatorQ +{ public: /** * Constructor @@ -106,6 +111,8 @@ class AttitudeEstimatorQ { void task_main(); + void print(); + private: static constexpr float _dt_max = 0.02; bool _task_should_exit = false; /**< if true, task should exit */ @@ -113,31 +120,47 @@ class AttitudeEstimatorQ { int _sensors_sub = -1; int _params_sub = -1; + int _vision_sub = -1; + int _mocap_sub = -1; int _global_pos_sub = -1; orb_advert_t _att_pub = nullptr; + orb_advert_t _ctrl_state_pub = nullptr; struct { param_t w_acc; param_t w_mag; + param_t w_ext_hdg; param_t w_gyro_bias; param_t mag_decl; param_t mag_decl_auto; param_t acc_comp; param_t bias_max; + param_t vibe_thresh; + param_t ext_hdg_mode; } _params_handles; /**< handles for interesting parameters */ float _w_accel = 0.0f; float _w_mag = 0.0f; + float _w_ext_hdg = 0.0f; float _w_gyro_bias = 0.0f; float _mag_decl = 0.0f; bool _mag_decl_auto = false; bool _acc_comp = false; float _bias_max = 0.0f; + float _vibration_warning_threshold = 1.0f; + hrt_abstime _vibration_warning_timestamp = 0; + int _ext_hdg_mode = 0; Vector<3> _gyro; Vector<3> _accel; Vector<3> _mag; + vision_position_estimate_s _vision = {}; + Vector<3> _vision_hdg; + + att_pos_mocap_s _mocap = {}; + Vector<3> _mocap_hdg; + Quaternion _q; Vector<3> _rates; Vector<3> _gyro_bias; @@ -145,9 +168,25 @@ class AttitudeEstimatorQ { vehicle_global_position_s _gpos = {}; Vector<3> _vel_prev; Vector<3> _pos_acc; + + DataValidatorGroup _voter_gyro; + DataValidatorGroup _voter_accel; + DataValidatorGroup _voter_mag; + + /* Low pass filter for attitude rates */ + math::LowPassFilter2p _lp_roll_rate; + math::LowPassFilter2p _lp_pitch_rate; + math::LowPassFilter2p _lp_yaw_rate; + hrt_abstime _vel_prev_t = 0; bool _inited = false; + bool _data_good = false; + bool _failsafe = false; + bool _vibration_warning = false; + bool _ext_hdg_good = false; + + int _mavlink_fd = -1; perf_counter_t _update_perf; perf_counter_t _loop_perf; @@ -156,26 +195,39 @@ class AttitudeEstimatorQ { int update_subscriptions(); - void init(); + bool init(); - void update(float dt); + bool update(float dt); }; -AttitudeEstimatorQ::AttitudeEstimatorQ() { +AttitudeEstimatorQ::AttitudeEstimatorQ() : + _voter_gyro(3), + _voter_accel(3), + _voter_mag(3), + _lp_roll_rate(250.0f, 18.0f), + _lp_pitch_rate(250.0f, 18.0f), + _lp_yaw_rate(250.0f, 10.0f) +{ + _voter_mag.set_timeout(200000); + _params_handles.w_acc = param_find("ATT_W_ACC"); _params_handles.w_mag = param_find("ATT_W_MAG"); + _params_handles.w_ext_hdg = param_find("ATT_W_EXT_HDG"); _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS"); _params_handles.mag_decl = param_find("ATT_MAG_DECL"); _params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A"); _params_handles.acc_comp = param_find("ATT_ACC_COMP"); _params_handles.bias_max = param_find("ATT_BIAS_MAX"); + _params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); + _params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M"); } /** * Destructor, also kills task. */ -AttitudeEstimatorQ::~AttitudeEstimatorQ() { +AttitudeEstimatorQ::~AttitudeEstimatorQ() +{ if (_control_task != -1) { /* task wakes up every 100ms or so at the longest */ _task_should_exit = true; @@ -189,7 +241,7 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() { /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -198,16 +250,17 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() { attitude_estimator_q::instance = nullptr; } -int AttitudeEstimatorQ::start() { +int AttitudeEstimatorQ::start() +{ ASSERT(_control_task == -1); /* start the task */ _control_task = px4_task_spawn_cmd("attitude_estimator_q", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2500, - (main_t)&AttitudeEstimatorQ::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2100, + (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -217,14 +270,28 @@ int AttitudeEstimatorQ::start() { return OK; } -void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) { +void AttitudeEstimatorQ::print() +{ + warnx("gyro status:"); + _voter_gyro.print(); + warnx("accel status:"); + _voter_accel.print(); + warnx("mag status:"); + _voter_mag.print(); +} + +void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) +{ attitude_estimator_q::instance->task_main(); } -void AttitudeEstimatorQ::task_main() { - warnx("started"); +void AttitudeEstimatorQ::task_main() +{ + _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); + + _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); + _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); - _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -232,17 +299,22 @@ void AttitudeEstimatorQ::task_main() { hrt_abstime last_time = 0; - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = _sensors_sub; fds[0].events = POLLIN; while (!_task_should_exit) { - int ret = poll(fds, 1, 1000); + int ret = px4_poll(fds, 1, 1000); + + if (_mavlink_fd < 0) { + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } if (ret < 0) { // Poll error, sleep and try again usleep(10000); continue; + } else if (ret == 0) { // Poll timeout, do nothing continue; @@ -252,23 +324,176 @@ void AttitudeEstimatorQ::task_main() { // Update sensors sensor_combined_s sensors; + if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) { - _gyro.set(sensors.gyro_rad_s); - _accel.set(sensors.accelerometer_m_s2); - _mag.set(sensors.magnetometer_ga); + // Feed validator with recent sensor data + + for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) { + + /* ignore empty fields */ + if (sensors.gyro_timestamp[i] > 0) { + + float gyro[3]; + + for (unsigned j = 0; j < 3; j++) { + if (sensors.gyro_integral_dt[i] > 0) { + gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6); + + } else { + /* fall back to angular rate */ + gyro[j] = sensors.gyro_rad_s[i * 3 + j]; + } + } + + _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); + } + + /* ignore empty fields */ + if (sensors.accelerometer_timestamp[i] > 0) { + _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + } + + /* ignore empty fields */ + if (sensors.magnetometer_timestamp[i] > 0) { + _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], + sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + } + } + + int best_gyro, best_accel, best_mag; + + // Get best measurement values + hrt_abstime curr_time = hrt_absolute_time(); + _gyro.set(_voter_gyro.get_best(curr_time, &best_gyro)); + _accel.set(_voter_accel.get_best(curr_time, &best_accel)); + _mag.set(_voter_mag.get_best(curr_time, &best_mag)); + + if (_accel.length() < 0.01f || _mag.length() < 0.01f) { + warnx("WARNING: degenerate accel / mag!"); + continue; + } + + _data_good = true; + + if (!_failsafe) { + uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR; + + if (_voter_gyro.failover_count() > 0) { + _failsafe = true; + flags = _voter_gyro.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!", + _voter_gyro.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); + } + + if (_voter_accel.failover_count() > 0) { + _failsafe = true; + flags = _voter_accel.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!", + _voter_accel.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); + } + + if (_voter_mag.failover_count() > 0) { + _failsafe = true; + flags = _voter_mag.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Mag #%i failure :%s%s%s%s%s!", + _voter_mag.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); + } + + if (_failsafe) { + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } + } + + if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { + + if (_vibration_warning_timestamp == 0) { + _vibration_warning_timestamp = curr_time; + + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { + _vibration_warning = true; + mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d", + (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), + (int)(100 * _voter_accel.get_vibration_factor(curr_time)), + (int)(100 * _voter_mag.get_vibration_factor(curr_time))); + } + + } else { + _vibration_warning_timestamp = 0; + } + } + + // Update vision and motion capture heading + bool vision_updated = false; + orb_check(_vision_sub, &vision_updated); + + bool mocap_updated = false; + orb_check(_mocap_sub, &mocap_updated); + + if (vision_updated) { + orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision); + math::Quaternion q(_vision.q); + + math::Matrix<3, 3> Rvis = q.to_dcm(); + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + // Rvis is Rwr (robot respect to world) while v is respect to world. + // Hence Rvis must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency + _vision_hdg = Rvis.transposed() * v; + } + + if (mocap_updated) { + orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap); + math::Quaternion q(_mocap.q); + math::Matrix<3, 3> Rmoc = q.to_dcm(); + + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + // Rmoc is Rwr (robot respect to world) while v is respect to world. + // Hence Rmoc must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency + _mocap_hdg = Rmoc.transposed() * v; + } + + // Check for timeouts on data + if (_ext_hdg_mode == 1) { + _ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000); + + } else if (_ext_hdg_mode == 2) { + _ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000); } bool gpos_updated; orb_check(_global_pos_sub, &gpos_updated); + if (gpos_updated) { orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos); + if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) { /* set magnetic declination automatically */ _mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon)); } } - if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f) { + if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) { /* position data is actual */ if (gpos_updated) { Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d); @@ -279,6 +504,7 @@ void AttitudeEstimatorQ::task_main() { /* calculate acceleration in body frame */ _pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt); } + _vel_prev_t = _gpos.timestamp; _vel_prev = vel; } @@ -291,15 +517,17 @@ void AttitudeEstimatorQ::task_main() { } // Time from previous iteration - uint64_t now = hrt_absolute_time(); - float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.0f; + hrt_abstime now = hrt_absolute_time(); + float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.00001f; last_time = now; if (dt > _dt_max) { dt = _dt_max; } - update(dt); + if (!update(dt)) { + continue; + } Vector<3> euler = _q.to_euler(); @@ -327,25 +555,62 @@ void AttitudeEstimatorQ::task_main() { memcpy(&att.R[0], R.data, sizeof(att.R)); att.R_valid = true; + att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time()); + att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time()); + att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time()); + if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); + } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } + + struct control_state_s ctrl_state = {}; + + ctrl_state.timestamp = sensors.timestamp; + + /* Attitude quaternions for control state */ + ctrl_state.q[0] = _q(0); + + ctrl_state.q[1] = _q(1); + + ctrl_state.q[2] = _q(2); + + ctrl_state.q[3] = _q(3); + + /* Attitude rates for control state */ + ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); + + ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); + + ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2)); + + /* Publish to control state topic */ + if (_ctrl_state_pub == nullptr) { + _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); + + } else { + orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state); + } } } -void AttitudeEstimatorQ::update_parameters(bool force) { +void AttitudeEstimatorQ::update_parameters(bool force) +{ bool updated = force; + if (!updated) { orb_check(_params_sub, &updated); } + if (updated) { parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); param_get(_params_handles.w_acc, &_w_accel); param_get(_params_handles.w_mag, &_w_mag); + param_get(_params_handles.w_ext_hdg, &_w_ext_hdg); param_get(_params_handles.w_gyro_bias, &_w_gyro_bias); float mag_decl_deg = 0.0f; param_get(_params_handles.mag_decl, &mag_decl_deg); @@ -357,10 +622,13 @@ void AttitudeEstimatorQ::update_parameters(bool force) { param_get(_params_handles.acc_comp, &acc_comp_int); _acc_comp = acc_comp_int != 0; param_get(_params_handles.bias_max, &_bias_max); + param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold); + param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode); } } -void AttitudeEstimatorQ::init() { +bool AttitudeEstimatorQ::init() +{ // Rotation matrix can be easily constructed from acceleration and mag field vectors // 'k' is Earth Z axis (Down) unit vector in body frame Vector<3> k = -_accel; @@ -381,41 +649,84 @@ void AttitudeEstimatorQ::init() { // Convert to quaternion _q.from_dcm(R); + _q.normalize(); + + if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && + PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) && + _q.length() > 0.95f && _q.length() < 1.05f) { + _inited = true; + + } else { + _inited = false; + } + + return _inited; } -void AttitudeEstimatorQ::update(float dt) { +bool AttitudeEstimatorQ::update(float dt) +{ if (!_inited) { - init(); - _inited = true; + + if (!_data_good) { + return false; + } + + return init(); } + Quaternion q_last = _q; + // Angular rate of correction Vector<3> corr; - // Magnetometer correction - // Project mag field vector to global frame and extract XY component - Vector<3> mag_earth = _q.conjugate(_mag); - float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl); - // Project magnetometer correction to body frame - corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; + if (_ext_hdg_mode > 0 && _ext_hdg_good) { + if (_ext_hdg_mode == 1) { + // Vision heading correction + // Project heading to global frame and extract XY component + Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg); + float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0))); + // Project correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg; + } + + if (_ext_hdg_mode == 2) { + // Mocap heading correction + // Project heading to global frame and extract XY component + Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg); + float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0))); + // Project correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg; + } + } + + if (_ext_hdg_mode == 0 || !_ext_hdg_good) { + // Magnetometer correction + // Project mag field vector to global frame and extract XY component + Vector<3> mag_earth = _q.conjugate(_mag); + float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl); + // Project magnetometer correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; + } // Accelerometer correction // Project 'k' unit vector of earth frame to body frame // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); // Optimized version with dropped zeros Vector<3> k( - 2.0f * (_q(1) * _q(3) - _q(0) * _q(2)), - 2.0f * (_q(2) * _q(3) + _q(0) * _q(1)), - (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3)) + 2.0f * (_q(1) * _q(3) - _q(0) * _q(2)), + 2.0f * (_q(2) * _q(3) + _q(0) * _q(1)), + (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3)) ); corr += (k % (_accel - _pos_acc).normalized()) * _w_accel; // Gyro bias estimation _gyro_bias += corr * (_w_gyro_bias * dt); + for (int i = 0; i < 3; i++) { _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max); } + _rates = _gyro + _gyro_bias; // Feed forward gyro @@ -425,52 +736,72 @@ void AttitudeEstimatorQ::update(float dt) { _q += _q.derivative(corr) * dt; // Normalize quaternion - _q.normalize(); // TODO! NaN protection??? + _q.normalize(); + + if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && + PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) { + // Reset quaternion to last good state + _q = q_last; + _rates.zero(); + _gyro_bias.zero(); + return false; + } + + return true; } -int attitude_estimator_q_main(int argc, char *argv[]) { +int attitude_estimator_q_main(int argc, char *argv[]) +{ if (argc < 1) { - errx(1, "usage: attitude_estimator_q {start|stop|status}"); + warnx("usage: attitude_estimator_q {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { if (attitude_estimator_q::instance != nullptr) { - errx(1, "already running"); + warnx("already running"); + return 1; } attitude_estimator_q::instance = new AttitudeEstimatorQ; if (attitude_estimator_q::instance == nullptr) { - errx(1, "alloc failed"); + warnx("alloc failed"); + return 1; } if (OK != attitude_estimator_q::instance->start()) { delete attitude_estimator_q::instance; attitude_estimator_q::instance = nullptr; - err(1, "start failed"); + warnx("start failed"); + return 1; } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { if (attitude_estimator_q::instance == nullptr) { - errx(1, "not running"); + warnx("not running"); + return 1; } delete attitude_estimator_q::instance; attitude_estimator_q::instance = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (attitude_estimator_q::instance) { - errx(0, "running"); + attitude_estimator_q::instance->print(); + warnx("running"); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index 730d7a4e8b7b..41b62f04e1a7 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -47,6 +47,7 @@ * @group Attitude Q estimator * @min 0 * @max 1 + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); @@ -56,15 +57,26 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); * @group Attitude Q estimator * @min 0 * @max 1 + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f); +/** + * Complimentary filter external heading weight + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f); + /** * Complimentary filter gyroscope bias weight * * @group Attitude Q estimator * @min 0 * @max 1 + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f); @@ -77,6 +89,7 @@ PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f); * * @group Attitude Q estimator * @unit degrees + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); @@ -89,6 +102,17 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); */ PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1); +/** + * External heading usage mode (from Motion capture/Vision) + * Set to 1 to use heading estimate from vision. + * Set to 2 to use heading from motion capture. + * + * @group Attitude Q estimator + * @min 0 + * @max 2 + */ +PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0); + /** * Enable acceleration compensation based on GPS * velocity. @@ -106,5 +130,16 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); * @min 0 * @max 2 * @unit rad/s + * @decimal 3 */ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); + +/** + * Threshold (of RMS) to warn about high vibration levels + * + * @group Attitude Q estimator + * @min 0.01 + * @max 10 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.2f); diff --git a/src/modules/attitude_estimator_so3/README b/src/modules/attitude_estimator_so3/README deleted file mode 100644 index 02ab66ee43d8..000000000000 --- a/src/modules/attitude_estimator_so3/README +++ /dev/null @@ -1,3 +0,0 @@ -Synopsis - - nsh> attitude_estimator_so3_comp start \ No newline at end of file diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp deleted file mode 100755 index 20bb1faf8a66..000000000000 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ /dev/null @@ -1,661 +0,0 @@ -/**************************************************************************** - * - * 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 attitude_estimator_so3_main.cpp - * - * @author Hyon Lim - * @author Anton Babushkin - * - * Implementation of nonlinear complementary filters on the SO(3). - * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. - * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * - * Theory of nonlinear complementary filters on the SO(3) is based on [1]. - * Quaternion realization of [1] is based on [2]. - * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * - * References - * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 - * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 - */ - -#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 - -#ifdef __cplusplus -extern "C" { -#endif -#include "attitude_estimator_so3_params.h" -#ifdef __cplusplus -} -#endif - -extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]); - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int attitude_estimator_so3_task; /**< Handle of deamon task / thread */ - -//! Auxiliary variables to reduce number of repeated operations -static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ -static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ -static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */ -static float q0q0, q0q1, q0q2, q0q3; -static float q1q1, q1q2, q1q3; -static float q2q2, q2q3; -static float q3q3; -static bool bFilterInit = false; - -/** - * Mainloop of attitude_estimator_so3. - */ -int attitude_estimator_so3_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* Function prototypes */ -float invSqrt(float number); -void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz); -void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n"); - exit(1); -} - -/** - * The attitude_estimator_so3 app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to px4_task_spawn_cmd(). - */ -int attitude_estimator_so3_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage("missing command"); - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - attitude_estimator_so3_task = px4_task_spawn_cmd("attitude_estimator_so3", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 14000, - attitude_estimator_so3_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - - while (thread_running){ - usleep(200000); - } - - warnx("stopped"); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("running"); - exit(0); - - } else { - warnx("not started"); - exit(1); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -//--------------------------------------------------------------------------------------------------- -// Fast inverse square-root -// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -float invSqrt(float number) -{ - volatile long i; - volatile float x, y; - volatile const float f = 1.5F; - - x = number * 0.5F; - y = number; - i = * (( long * ) &y); - i = 0x5f375a86 - ( i >> 1 ); - y = * (( float * ) &i); - y = y * ( f - ( x * y * y ) ); - return y; -} - -//! Using accelerometer, sense the gravity vector. -//! Using magnetometer, sense yaw. -void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; - - // auxillary variables to reduce number of repeated operations, for 1st pass - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; -} - -void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) -{ - float recipNorm; - float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; - - // Make filter converge to initial solution faster - // This function assumes you are in static position. - // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen. - if(bFilterInit == false) { - NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz); - bFilterInit = true; - } - - //! If magnetometer measurement is available, use it. - if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { - float hx, hy, hz, bx, bz; - float halfwx, halfwy, halfwz; - - // Normalise magnetometer measurement - // Will sqrt work better? PX4 system is powerful enough? - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; - - // Reference direction of Earth's magnetic field - hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); - hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); - hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2); - bx = sqrt(hx * hx + hy * hy); - bz = hz; - - // Estimated direction of magnetic field - halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); - halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); - halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); - - // Error is sum of cross product between estimated direction and measured direction of field vectors - halfex += (my * halfwz - mz * halfwy); - halfey += (mz * halfwx - mx * halfwz); - halfez += (mx * halfwy - my * halfwx); - } - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - float halfvx, halfvy, halfvz; - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Estimated direction of gravity and magnetic field - halfvx = q1q3 - q0q2; - halfvy = q0q1 + q2q3; - halfvz = q0q0 - 0.5f + q3q3; - - // Error is sum of cross product between estimated direction and measured direction of field vectors - halfex += ay * halfvz - az * halfvy; - halfey += az * halfvx - ax * halfvz; - halfez += ax * halfvy - ay * halfvx; - } - - // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer - if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) { - // Compute and apply integral feedback if enabled - if(twoKi > 0.0f) { - gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki - gyro_bias[1] += twoKi * halfey * dt; - gyro_bias[2] += twoKi * halfez * dt; - - // apply integral feedback - gx += gyro_bias[0]; - gy += gyro_bias[1]; - gz += gyro_bias[2]; - } - else { - gyro_bias[0] = 0.0f; // prevent integral windup - gyro_bias[1] = 0.0f; - gyro_bias[2] = 0.0f; - } - - // Apply proportional feedback - gx += twoKp * halfex; - gy += twoKp * halfey; - gz += twoKp * halfez; - } - - //! Integrate rate of change of quaternion -#if 0 - gx *= (0.5f * dt); // pre-multiply common factors - gy *= (0.5f * dt); - gz *= (0.5f * dt); -#endif - - // Time derivative of quaternion. q_dot = 0.5*q\otimes omega. - //! q_k = q_{k-1} + dt*\dot{q} - //! \dot{q} = 0.5*q \otimes P(\omega) - dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz); - dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy); - dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx); - dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx); - - q0 += dt*dq0; - q1 += dt*dq1; - q2 += dt*dq2; - q3 += dt*dq3; - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; -} - -/* - * Nonliner complementary filter on SO(3), attitude estimator main function. - * - * Estimates the attitude once started. - * - * @param argc number of commandline arguments (plus command name) - * @param argv strings containing the arguments - */ -int attitude_estimator_so3_thread_main(int argc, char *argv[]) -{ - //! Time constant - float dt = 0.005f; - - /* output euler angles */ - float euler[3] = {0.0f, 0.0f, 0.0f}; - - /* Initialization */ - float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */ - float acc[3] = {0.0f, 0.0f, 0.0f}; - float gyro[3] = {0.0f, 0.0f, 0.0f}; - float mag[3] = {0.0f, 0.0f, 0.0f}; - - warnx("main thread started"); - - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - - //! Initialize attitude vehicle uORB message. - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - - uint64_t last_data = 0; - uint64_t last_measurement = 0; - - /* subscribe to raw data */ - int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ - orb_set_interval(sub_raw, 3); - - /* subscribe to param changes */ - int sub_params = orb_subscribe(ORB_ID(parameter_update)); - - /* subscribe to control mode */ - int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); - - /* advertise attitude */ - orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); - - int loopcounter = 0; - - thread_running = true; - - // XXX write this out to perf regs - - /* keep track of sensor updates */ - uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - - struct attitude_estimator_so3_params so3_comp_params; - struct attitude_estimator_so3_param_handles so3_comp_param_handles; - - /* initialize parameter handles */ - parameters_init(&so3_comp_param_handles); - parameters_update(&so3_comp_param_handles, &so3_comp_params); - - uint64_t start_time = hrt_absolute_time(); - bool initialized = false; - bool state_initialized = false; - - float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; - unsigned offset_count = 0; - - /* register the perf counter */ - perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3"); - - /* Main loop*/ - while (!thread_should_exit) { - - struct pollfd fds[2]; - fds[0].fd = sub_raw; - fds[0].events = POLLIN; - fds[1].fd = sub_params; - fds[1].events = POLLIN; - int ret = poll(fds, 2, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - - if (!control_mode.flag_system_hil_enabled) { - warnx("WARNING: Not getting sensors - sensor app running?"); - } - } else { - /* only update parameters if they changed */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), sub_params, &update); - - /* update parameters */ - parameters_update(&so3_comp_param_handles, &so3_comp_params); - } - - /* only run filter if sensor values changed */ - if (fds[0].revents & POLLIN) { - - /* get latest measurements */ - orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - - if (!initialized) { - - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; - - if (hrt_absolute_time() > start_time + 3000000l) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]); - } - - } else { - - perf_begin(so3_comp_loop_perf); - - /* Calculate data time difference in seconds */ - dt = (raw.timestamp - last_measurement) / 1000000.0f; - last_measurement = raw.timestamp; - - /* Fill in gyro measurements */ - if (sensor_last_timestamp[0] != raw.timestamp) { - sensor_last_timestamp[0] = raw.timestamp; - } - - gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; - gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; - gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; - - /* update accelerometer measurements */ - if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { - sensor_last_timestamp[1] = raw.accelerometer_timestamp; - } - - acc[0] = raw.accelerometer_m_s2[0]; - acc[1] = raw.accelerometer_m_s2[1]; - acc[2] = raw.accelerometer_m_s2[2]; - - /* update magnetometer measurements */ - if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { - sensor_last_timestamp[2] = raw.magnetometer_timestamp; - } - - mag[0] = raw.magnetometer_ga[0]; - mag[1] = raw.magnetometer_ga[1]; - mag[2] = raw.magnetometer_ga[2]; - - /* initialize with good values once we have a reasonable dt estimate */ - if (!state_initialized && dt < 0.05f && dt > 0.001f) { - state_initialized = true; - warnx("state initialized"); - } - - /* do not execute the filter if not initialized */ - if (!state_initialized) { - continue; - } - - // NOTE : Accelerometer is reversed. - // Because proper mount of PX4 will give you a reversed accelerometer readings. - NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], - -acc[0], -acc[1], -acc[2], - mag[0], mag[1], mag[2], - so3_comp_params.Kp, - so3_comp_params.Ki, - dt); - - // Convert q->R, This R converts inertial frame to body frame. - Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 - Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12 - Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13 - Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21 - Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 - Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23 - Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31 - Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32 - Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 - - //1-2-3 Representation. - //Equation (290) - //Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel. - // Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix. - euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll - euler[1] = -asinf(Rot_matrix[2]); //! Pitch - euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]); //! Yaw - - /* swap values for next iteration, check for fatal inputs */ - if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { - // Publish only finite euler angles - att.roll = euler[0] - so3_comp_params.roll_off; - att.pitch = euler[1] - so3_comp_params.pitch_off; - att.yaw = euler[2] - so3_comp_params.yaw_off; - } else { - /* due to inputs or numerical failure the output is invalid, skip it */ - // Due to inputs or numerical failure the output is invalid - warnx("infinite euler angles, rotation matrix:"); - warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]); - warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]); - warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]); - // Don't publish anything - continue; - } - - if (last_data > 0 && raw.timestamp > last_data + 12000) { - warnx("sensor data missed"); - } - - last_data = raw.timestamp; - - /* send out */ - att.timestamp = raw.timestamp; - - // Quaternion - att.q[0] = q0; - att.q[1] = q1; - att.q[2] = q2; - att.q[3] = q3; - att.q_valid = true; - - // Euler angle rate. But it needs to be investigated again. - /* - att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); - att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); - att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3); - */ - att.rollspeed = gyro[0]; - att.pitchspeed = gyro[1]; - att.yawspeed = gyro[2]; - - att.rollacc = 0; - att.pitchacc = 0; - att.yawacc = 0; - - /* TODO: Bias estimation required */ - memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); - - /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(float)*9); - att.R_valid = true; - - // Publish - if (att_pub != nullptr) { - orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); - } else { - warnx("NaN in roll/pitch/yaw estimate!"); - orb_advertise(ORB_ID(vehicle_attitude), &att); - } - - perf_end(so3_comp_loop_perf); - } - } - } - - loopcounter++; - } - - thread_running = false; - - return 0; -} diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c deleted file mode 100755 index 0c8d522b4d3a..000000000000 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c +++ /dev/null @@ -1,86 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Hyon Lim - * Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file attitude_estimator_so3_params.c - * - * Parameters for nonlinear complementary filters on the SO(3). - */ - -#include "attitude_estimator_so3_params.h" - -/* This is filter gain for nonlinear SO3 complementary filter */ -/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place. - Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP. - If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which - will compensate gyro bias which depends on temperature and vibration of your vehicle */ -PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time. - //! You can set this gain higher if you want more fast response. - //! But note that higher gain will give you also higher overshoot. -PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change) - //! This gain is depend on your vehicle status. - -/* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f); - -int parameters_init(struct attitude_estimator_so3_param_handles *h) -{ - /* Filter gain parameters */ - h->Kp = param_find("SO3_COMP_KP"); - h->Ki = param_find("SO3_COMP_KI"); - - /* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */ - h->roll_off = param_find("SO3_ROLL_OFFS"); - h->pitch_off = param_find("SO3_PITCH_OFFS"); - h->yaw_off = param_find("SO3_YAW_OFFS"); - - return OK; -} - -int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p) -{ - /* Update filter gain */ - param_get(h->Kp, &(p->Kp)); - param_get(h->Ki, &(p->Ki)); - - /* Update attitude offset */ - param_get(h->roll_off, &(p->roll_off)); - param_get(h->pitch_off, &(p->pitch_off)); - param_get(h->yaw_off, &(p->yaw_off)); - - return OK; -} diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk deleted file mode 100644 index 7b2e206cc5c1..000000000000 --- a/src/modules/attitude_estimator_so3/module.mk +++ /dev/null @@ -1,12 +0,0 @@ -# -# Attitude estimator (Nonlinear SO(3) complementary Filter) -# - -MODULE_COMMAND = attitude_estimator_so3 - -SRCS = attitude_estimator_so3_main.cpp \ - attitude_estimator_so3_params.c - -MODULE_STACKSIZE = 1200 - -EXTRACXXFLAGS = -Wno-float-equal diff --git a/src/modules/bottle_drop/CMakeLists.txt b/src/modules/bottle_drop/CMakeLists.txt new file mode 100644 index 000000000000..7c77cac34096 --- /dev/null +++ b/src/modules/bottle_drop/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__bottle_drop + MAIN bottle_drop + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + bottle_drop.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index c00e73ecee0f..8d2621ff6270 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -226,11 +226,11 @@ BottleDrop::start() /* start the task */ _main_task = px4_task_spawn_cmd("bottle_drop", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT + 15, - 1500, - (main_t)&BottleDrop::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT + 15, + 1500, + (main_t)&BottleDrop::task_main_trampoline, + nullptr); if (_main_task < 0) { warn("task start failed"); @@ -256,6 +256,7 @@ BottleDrop::open_bay() if (_doors_opened == 0) { _doors_opened = hrt_absolute_time(); } + warnx("open doors"); actuators_publish(); @@ -326,8 +327,10 @@ BottleDrop::actuators_publish() } else { _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); + if (_actuator_pub != nullptr) { return OK; + } else { return -1; } @@ -338,7 +341,7 @@ void BottleDrop::task_main() { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); _command_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -459,6 +462,7 @@ BottleDrop::task_main() } orb_check(vehicle_global_position_sub, &updated); + if (updated) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); @@ -478,12 +482,14 @@ BottleDrop::task_main() // Get wind estimate orb_check(_wind_estimate_sub, &updated); + if (updated) { orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); } // Get vehicle position orb_check(vehicle_global_position_sub, &updated); + if (updated) { // copy global position orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); @@ -491,6 +497,7 @@ BottleDrop::task_main() // Get parameter updates orb_check(parameter_update_sub, &updated); + if (updated) { // copy global position orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); @@ -502,6 +509,7 @@ BottleDrop::task_main() } orb_check(_command_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); handle_command(&_command); @@ -515,25 +523,27 @@ BottleDrop::task_main() // Distance to drop position and angle error to approach vector // are relevant in all states greater than target valid (which calculates these positions) if (_drop_state > DROP_STATE_TARGET_VALID) { - distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, + _drop_position.lon)); float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); - float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, + flight_vector_e.lon); approach_error = _wrap_pi(ground_direction - approach_direction); if (counter % 90 == 0) { - mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, + (unsigned)math::degrees(approach_error)); } } switch (_drop_state) { - case DROP_STATE_INIT: - // do nothing - break; + case DROP_STATE_INIT: + // do nothing + break; - case DROP_STATE_TARGET_VALID: - { + case DROP_STATE_TARGET_VALID: { az = g; // acceleration in z direction[m/s^2] vz = 0; // velocity in z direction [m/s] @@ -626,27 +636,30 @@ BottleDrop::task_main() _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } - float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); - mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, + flight_vector_e.lon); + mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, + (unsigned)math::degrees(approach_direction + M_PI_F)); _drop_state = DROP_STATE_TARGET_SET; } break; - case DROP_STATE_TARGET_SET: - { - float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); + case DROP_STATE_TARGET_SET: { + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, + flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } else { // We're close enough - open the bay distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); if (isfinite(distance_real) && distance_real < distance_open_door && - fabsf(approach_error) < math::radians(20.0f)) { + fabsf(approach_error) < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); @@ -655,52 +668,55 @@ BottleDrop::task_main() } break; - case DROP_STATE_BAY_OPEN: - { - if (_drop_approval) { - map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); - x_f = x_l + _global_pos.vel_n * dt_runs; - y_f = y_l + _global_pos.vel_e * dt_runs; - map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); - - if (isfinite(distance_real) && - (distance_real < precision) && ((distance_real < future_distance))) { - drop(); - _drop_state = DROP_STATE_DROPPED; - mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); - } else { - - float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); - - if (distance_wp2 < distance_real) { - _onboard_mission.current_seq = 0; - orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); - } + case DROP_STATE_BAY_OPEN: { + if (_drop_approval) { + map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); + + if (isfinite(distance_real) && + (distance_real < precision) && ((distance_real < future_distance))) { + drop(); + _drop_state = DROP_STATE_DROPPED; + mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); + + } else { + + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, + flight_vector_e.lon); + + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } } } - break; - - case DROP_STATE_DROPPED: - /* 2s after drop, reset and close everything again */ - if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { - _drop_state = DROP_STATE_INIT; - _drop_approval = false; - lock_release(); - close_bay(); - mavlink_log_info(_mavlink_fd, "#audio: closing bay"); - - // remove onboard mission - _onboard_mission.current_seq = -1; - _onboard_mission.count = 0; - orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); - } - break; + } + break; + + case DROP_STATE_DROPPED: + + /* 2s after drop, reset and close everything again */ + if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { + _drop_state = DROP_STATE_INIT; + _drop_approval = false; + lock_release(); + close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); - case DROP_STATE_BAY_CLOSED: - // do nothing - break; + // remove onboard mission + _onboard_mission.current_seq = -1; + _onboard_mission.count = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + + break; + + case DROP_STATE_BAY_CLOSED: + // do nothing + break; } counter++; @@ -726,6 +742,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) { switch (cmd->command) { case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: + /* * param1 and param2 set to 1: open and drop * param1 set to 1: open @@ -775,7 +792,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _target_position.alt = cmd->param7; _drop_state = DROP_STATE_TARGET_VALID; mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, - (double)_target_position.lon, (double)_target_position.alt); + (double)_target_position.lon, (double)_target_position.alt); map_projection_init(&ref, _target_position.lat, _target_position.lon); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); break; diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt new file mode 100644 index 000000000000..4239797e0a2a --- /dev/null +++ b/src/modules/commander/CMakeLists.txt @@ -0,0 +1,60 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +set(MODULE_CFLAGS -Os) +if(${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=2450) +endif() +px4_add_module( + MODULE modules__commander + MAIN commander + STACK 4096 + COMPILE_FLAGS + ${MODULE_CFLAGS} + -Os + SRCS + commander.cpp + state_machine_helper.cpp + commander_helper.cpp + calibration_routines.cpp + accelerometer_calibration.cpp + gyro_calibration.cpp + mag_calibration.cpp + baro_calibration.cpp + rc_calibration.cpp + airspeed_calibration.cpp + esc_calibration.cpp + PreflightCheck.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 9d38e8179a1b..54847fae2806 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -71,7 +71,50 @@ namespace Commander { -static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) + +static int check_calibration(int fd, const char* param_template, int &devid); + +int check_calibration(int fd, const char* param_template, int &devid) +{ + bool calibration_found; + + /* new style: ask device for calibration state */ + int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); + + calibration_found = (ret == OK); + + devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + + char s[20]; + int instance = 0; + + /* old style transition: check param values */ + while (!calibration_found) { + sprintf(s, param_template, instance); + param_t parm = param_find(s); + + /* if the calibration param is not present, abort */ + if (parm == PARAM_INVALID) { + break; + } + + /* if param get succeeds */ + int calibration_devid; + if (!param_get(parm, &(calibration_devid))) { + + /* if the devid matches, exit early */ + if (devid == calibration_devid) { + calibration_found = true; + break; + } + } + instance++; + } + + return !calibration_found; +} + +static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -81,22 +124,22 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); + } } return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_MAG%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id); - if (devid != calibration_devid) { - mavlink_and_console_log_critical(mavlink_fd, + if (ret) { + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); + } success = false; goto out; } @@ -104,8 +147,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); + } success = false; goto out; } @@ -115,7 +160,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) return success; } -static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic) +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail) { bool success = true; @@ -125,22 +170,22 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); + } } return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_ACC%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id); - if (devid != calibration_devid) { - mavlink_and_console_log_critical(mavlink_fd, + if (ret) { + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); + } success = false; goto out; } @@ -148,12 +193,15 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); + } success = false; goto out; } +#ifdef __PX4_NUTTX if (dynamic) { /* check measurement result range */ struct accel_report acc; @@ -164,25 +212,30 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + } /* this is frickin' fatal */ success = false; goto out; } } else { - mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + } /* this is frickin' fatal */ success = false; goto out; } } +#endif out: px4_close(fd); return success; } -static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -192,22 +245,22 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); + } } return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_GYRO%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id); - if (devid != calibration_devid) { - mavlink_and_console_log_critical(mavlink_fd, + if (ret) { + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); + } success = false; goto out; } @@ -215,8 +268,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); + } success = false; goto out; } @@ -226,7 +281,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) return success; } -static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -236,18 +291,34 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); + } } return false; } + device_id = -1000; + + // TODO: There is no baro calibration yet, since no external baros exist + // int ret = check_calibration(fd, "CAL_BARO%u_ID"); + + // if (ret) { + // mavlink_and_console_log_critical(mavlink_fd, + // "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance); + // success = false; + // goto out; + // } + +//out: + px4_close(fd); return success; } -static bool airspeedCheck(int mavlink_fd, bool optional) +static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail) { bool success = true; int ret; @@ -257,32 +328,36 @@ static bool airspeedCheck(int mavlink_fd, bool optional) if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + } success = false; goto out; } if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { - mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + } // XXX do not make this fatal yet } out: - close(fd); + px4_close(fd); return success; } -static bool gnssCheck(int mavlink_fd) +static bool gnssCheck(int mavlink_fd, bool report_fail) { bool success = true; int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); //Wait up to 2000ms to allow the driver to detect a GNSS receiver module - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = gpsSub; fds[0].events = POLLIN; - if(poll(fds, 1, 2000) <= 0) { + if(px4_poll(fds, 1, 2000) <= 0) { success = false; } else { @@ -294,84 +369,158 @@ static bool gnssCheck(int mavlink_fd) } //Report failure to detect module - if(!success) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + if (!success) { + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + } } - close(gpsSub); + px4_close(gpsSub); return success; } bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, - bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic) + bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures) { bool failed = false; /* ---- MAG ---- */ if (checkMag) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_MAG_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_mag_count; i++) { bool required = (i < max_mandatory_mag_count); + int device_id = -1; - if (!magnometerCheck(mavlink_fd, i, !required) && required) { + if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found && prime_id != 0) { + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found"); + } + failed = true; } } /* ---- ACCEL ---- */ if (checkAcc) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_ACC_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { bool required = (i < max_mandatory_accel_count); + int device_id = -1; - if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) { + if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found && prime_id != 0) { + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found"); + } + failed = true; } } /* ---- GYRO ---- */ if (checkGyro) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_GYRO_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_gyro_count; i++) { bool required = (i < max_mandatory_gyro_count); + int device_id = -1; - if (!gyroCheck(mavlink_fd, i, !required) && required) { + if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found && prime_id != 0) { + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found"); + } + failed = true; } } /* ---- BARO ---- */ if (checkBaro) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_BARO_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_baro_count; i++) { bool required = (i < max_mandatory_baro_count); + int device_id = -1; - if (!baroCheck(mavlink_fd, i, !required) && required) { + if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + // TODO there is no logic in place to calibrate the primary baro yet + // // check if the primary device is present + if (!prime_found && prime_id != 0) { + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational"); + } + failed = true; } } /* ---- AIRSPEED ---- */ if (checkAirspeed) { - if (!airspeedCheck(mavlink_fd, true)) { + if (!airspeedCheck(mavlink_fd, true, reportFailures)) { failed = true; } } /* ---- RC CALIBRATION ---- */ if (checkRC) { - if (rc_calibration_check(mavlink_fd) != OK) { + if (rc_calibration_check(mavlink_fd, reportFailures) != OK) { + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed"); + } failed = true; } } /* ---- Global Navigation Satellite System receiver ---- */ - if(checkGNSS) { - if(!gnssCheck(mavlink_fd)) { + if (checkGNSS) { + if (!gnssCheck(mavlink_fd, reportFailures)) { failed = true; } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index b6423a4d9ac5..058d6b895653 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -67,7 +67,7 @@ namespace Commander * true if the GNSS receiver should be checked **/ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, - bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false); + bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures = false); const unsigned max_mandatory_gyro_count = 1; const unsigned max_optional_gyro_count = 3; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 87d9469a1dda..d7a58e62542a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -156,6 +156,10 @@ static const int ERROR = -1; static const char *sensor_name = "accel"; +static int32_t device_id[max_accel_sens]; +static int device_prio_max = 0; +static int32_t device_id_primary = 0; + calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); int mat_invert3(float src[3][3], float dst[3][3]); @@ -172,7 +176,6 @@ typedef struct { int do_accel_calibration(int mavlink_fd) { int fd; - int32_t device_id[max_accel_sens]; mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); @@ -259,6 +262,8 @@ int do_accel_calibration(int mavlink_fd) bool failed = false; + failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); + /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); @@ -370,6 +375,15 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); timestamps[i] = arp.timestamp; + + // Get priority + int32_t prio; + orb_priority(worker_data.subs[i], &prio); + + if (prio > device_prio_max) { + device_prio_max = prio; + device_id_primary = device_id[i]; + } } if (result == calibrate_return_ok) { @@ -561,6 +575,7 @@ int do_level_calibration(int mavlink_fd) { const unsigned cal_time = 5; const unsigned cal_hz = 100; const unsigned settle_time = 30; + bool success = false; int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); @@ -599,7 +614,15 @@ int do_level_calibration(int mavlink_fd) { start = hrt_absolute_time(); // average attitude for 5 seconds while(hrt_elapsed_time(&start) < cal_time * 1000000) { - px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + if (pollret <= 0) { + // attitude estimator is not running + mavlink_and_console_log_critical(mavlink_fd, "attitude estimator not running - check system boot"); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level"); + goto out; + } + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); roll_mean += att.roll; pitch_mean += att.pitch; @@ -608,7 +631,6 @@ int do_level_calibration(int mavlink_fd) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); - bool success = false; if (counter > (cal_time * cal_hz / 2 )) { roll_mean /= counter; pitch_mean /= counter; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index f2f4f0c64fd1..4139a949e0cb 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -68,19 +68,19 @@ static const char *sensor_name = "dpress"; static void feedback_calibration_failed(int mavlink_fd) { sleep(5); - mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } int do_airspeed_calibration(int mavlink_fd) { int result = OK; unsigned calibration_counter = 0; - const unsigned maxcount = 3000; + const unsigned maxcount = 2400; /* give directions */ - mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); - const unsigned calibration_count = 2000; + const unsigned calibration_count = (maxcount * 2) / 3; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -101,7 +101,7 @@ int do_airspeed_calibration(int mavlink_fd) paramreset_successful = true; } else { - mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed"); } px4_close(fd); @@ -115,18 +115,18 @@ int do_airspeed_calibration(int mavlink_fd) float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { - mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd"); goto error_return; } /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } } - mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind"); usleep(500 * 1000); while (calibration_counter < calibration_count) { @@ -149,7 +149,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { @@ -167,14 +167,14 @@ int do_airspeed_calibration(int mavlink_fd) airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed"); } px4_close(fd_scale); } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } @@ -183,7 +183,7 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); goto error_return; } @@ -192,22 +192,22 @@ int do_airspeed_calibration(int mavlink_fd) goto error_return; } - mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - mavlink_log_critical(mavlink_fd, "[cal] Create airflow now"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now"); calibration_counter = 0; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { - if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { - goto error_return; - } - + if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { + goto error_return; + } + /* wait blocking for new data */ px4_pollfd_struct_t fds[1]; fds[0].fd = diff_pres_sub; @@ -222,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -230,26 +230,26 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!"); + mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!"); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ diff_pres_offset = 0.0f; if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } /* save */ - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0); (void)param_save_default(); feedback_calibration_failed(mavlink_fd); goto error_return; } else { - mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } @@ -266,9 +266,9 @@ int do_airspeed_calibration(int mavlink_fd) goto error_return; } - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); - mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); tune_neutral(true); normal_return: diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 53775ffe4f7a..1dbc5b6dbfb4 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -49,7 +49,7 @@ // instead of visual calibration until such a time as QGC is update to the new version. // The number in the cal started message is used to indicate the version stamp for the current calibration code. -#define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s" +#define CAL_QGC_STARTED_MSG "[cal] calibration started: 2 %s" #define CAL_QGC_DONE_MSG "[cal] calibration done: %s" #define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s" #define CAL_QGC_WARNING_MSG "[cal] calibration warning: %s" diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index df243df1a93d..4e4ffd3c8188 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -244,7 +244,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub const float normal_still_thr = 0.25; // normal still threshold float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2); float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2 - hrt_abstime still_time = lenient_still_position ? 1000000 : 1500000; // still time required in us + hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us px4_pollfd_struct_t fds[1]; fds[0].fd = accel_sub; @@ -325,7 +325,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub /* not still, reset still start time */ if (t_still != 0) { mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); - usleep(500000); + usleep(200000); t_still = 0; } } @@ -470,8 +470,8 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, /* inform user about already handled side */ if (side_data_collected[orient]) { orientation_failures++; - mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient)); - mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side"); + mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient)); + mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side"); continue; } @@ -489,11 +489,11 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, // Note that this side is complete side_data_collected[orient] = true; tune_neutral(true); - usleep(500000); + usleep(200000); } if (sub_accel >= 0) { - close(sub_accel); + px4_close(sub_accel); } return result; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fd33eca354af..49e6459b031f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -37,14 +37,15 @@ * * @author Petri Tanskanen * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin */ #include #include #include +#include #include #include #include @@ -57,9 +58,6 @@ #include #include //#include -#ifndef __PX4_QURT -#include -#endif #include #include #include @@ -74,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -93,7 +92,8 @@ #include #include #include - #include +#include +#include #include #include @@ -108,6 +108,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -147,8 +148,15 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 +#define HOTPLUG_SENS_TIMEOUT (8 * 1000 * 1000) /**< wait for hotplug sensors to come online for upto 10 seconds */ + #define PRINT_INTERVAL 5000000 -#define PRINT_MODE_REJECT_INTERVAL 2000000 +#define PRINT_MODE_REJECT_INTERVAL 10000000 + +#define INAIR_RESTART_HOLDOFF_INTERVAL 1000000 + +#define HIL_ID_MIN 1000 +#define HIL_ID_MAX 1999 enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ @@ -174,13 +182,14 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */ +static bool _usb_telemetry_active = false; +static hrt_abstime commander_boot_timestamp = 0; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; +static uint64_t _inair_last_time = 0; -static float takeoff_alt = 5.0f; -static int parachute_enabled = 0; static float eph_threshold = 5.0f; static float epv_threshold = 10.0f; @@ -189,6 +198,12 @@ static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; static struct offboard_control_mode_s offboard_control_mode; +static struct home_position_s _home; + +static unsigned _last_mission_instance = 0; +static manual_control_setpoint_s _last_sp_man; + +static struct vtol_vehicle_status_s vtol_status; /** * The daemon app only briefly exists to start @@ -212,7 +227,7 @@ void usage(const char *reason); */ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - orb_advert_t *home_pub); + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -229,6 +244,8 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct ma void set_control_mode(); +bool stabilization_required(); + void print_reject_mode(struct vehicle_status_s *current_status, const char *msg); void print_reject_arm(const char *msg); @@ -245,7 +262,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed * time the vehicle is armed with a good GPS fix. **/ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, - const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, + const vehicle_attitude_s &attitude); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -254,6 +272,15 @@ void *commander_low_prio_loop(void *arg); void answer_command(struct vehicle_command_s &cmd, unsigned result); +/** + * check whether autostart ID is in the reserved range for HIL setups + */ +bool is_hil_setup(int id); + +bool is_hil_setup(int id) { + return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX); +} + int commander_main(int argc, char *argv[]) { @@ -265,7 +292,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("commander already running"); + warnx("already running"); /* this is not an error */ return 0; } @@ -274,15 +301,22 @@ int commander_main(int argc, char *argv[]) daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3400, + 3600, commander_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + (char * const *)&argv[0]); + + unsigned constexpr max_wait_us = 1000000; + unsigned constexpr max_wait_steps = 2000; - while (!thread_running) { - usleep(200); + unsigned i; + for (i = 0; i < max_wait_steps; i++) { + usleep(max_wait_us / max_wait_steps); + if (thread_running) { + break; + } } - return 0; + return !(i < max_wait_steps); } if (!strcmp(argv[1], "stop")) { @@ -328,13 +362,15 @@ int commander_main(int argc, char *argv[]) calib_ret = do_level_calibration(mavlink_fd); } else if (!strcmp(argv[2], "esc")) { calib_ret = do_esc_calibration(mavlink_fd, &armed); + } else if (!strcmp(argv[2], "airspeed")) { + calib_ret = do_airspeed_calibration(mavlink_fd); } else { warnx("argument %s unsupported.", argv[2]); } if (calib_ret) { warnx("calibration failed, exiting."); - return 0; + return 1; } else { return 0; } @@ -344,25 +380,34 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "check")) { - int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); - int checkres = prearm_check(&status, mavlink_fd_local); - close(mavlink_fd_local); - warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); + int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); + int checkres = 0; + checkres = preflight_check(&status, mavlink_fd_local, false, true); + warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); + checkres = preflight_check(&status, mavlink_fd_local, true, true); + warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); + px4_close(mavlink_fd_local); return 0; } if (!strcmp(argv[1], "arm")) { - int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); - arm_disarm(true, mavlink_fd_local, "command line"); - close(mavlink_fd_local); + int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); + if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) { + warnx("note: not updating home position on commandline arming!"); + } else { + warnx("arming failed"); + } + px4_close(mavlink_fd_local); return 0; } if (!strcmp(argv[1], "disarm")) { - int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); - arm_disarm(false, mavlink_fd_local, "command line"); - close(mavlink_fd_local); - return 0; + int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); + if (TRANSITION_DENIED == arm_disarm(false, mavlink_fd_local, "command line")) { + warnx("rejected disarm"); + } + px4_close(mavlink_fd_local); + return 0; } usage("unrecognized command"); @@ -371,18 +416,21 @@ int commander_main(int argc, char *argv[]) void usage(const char *reason) { - if (reason) { - fprintf(stderr, "%s\n", reason); + if (reason && *reason > 0) { + PX4_INFO("%s", reason); } - fprintf(stderr, "usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n"); + PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n"); } void print_status() { warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion"); - warnx("usb powered: %s", (status.usb_connected) ? "yes" : "no"); + warnx("power: USB: %s, BRICK: %s", (status.usb_connected) ? "[OK]" : "[NO]", + (status.condition_power_input_valid) ? " [OK]" : "[NO]"); warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", _home.lat, _home.lon, (double)_home.alt); + warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z); /* read all relevant states */ int state_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -425,7 +473,7 @@ void print_status() break; } - close(state_sub); + px4_close(state_sub); warnx("arming: %s", armed_str); @@ -437,6 +485,13 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char { transition_result_t arming_res = TRANSITION_NOT_CHANGED; + // For HIL platforms, require that simulated sensors are connected + if (arm && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL && + is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming"); + return TRANSITION_DENIED; + } + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, @@ -454,7 +509,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, - struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) + struct home_position_s *home, struct vehicle_global_position_s *global_pos, + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) @@ -480,7 +536,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state - arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED; + + arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command"); + + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + if (cmd_arm && (arming_ret == TRANSITION_CHANGED) && + (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) { + + commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude); + } if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ @@ -504,6 +569,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* ACRO */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { + /* STABILIZED */ + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); @@ -521,6 +590,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* STABILIZED */ + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB); + } else { /* MANUAL */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } @@ -532,6 +604,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + + if (arming_ret == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "Rejecting arming cmd"); + } + + if (main_ret == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "Rejecting mode switch cmd"); + } } } break; @@ -733,6 +813,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN: case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: @@ -741,6 +822,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE: + case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL: + case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION: /* ignore commands that handled in low prio loop */ break; @@ -770,7 +853,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s * time the vehicle is armed with a good GPS fix. **/ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, - const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, + const vehicle_attitude_s &attitude) { //Need global position fix to be able to set home if (!status.condition_global_position_valid) { @@ -792,6 +876,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & home.y = localPosition.y; home.z = localPosition.z; + home.yaw = attitude.yaw; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -805,7 +891,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & //Play tune first time we initialize HOME if (!status.condition_home_position_valid) { - tune_positive(true); + tune_home_set(true); } /* mark home position as set */ @@ -817,15 +903,32 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; + bool sensor_fail_tune_played = false; bool arm_tune_played = false; + bool was_landed = true; bool was_armed = false; + bool startup_in_hil = false; + +#ifdef __PX4_NUTTX + /* NuttX indicates 3 arguments when only 2 are present */ + argc -= 1; +#endif + + if (argc > 2) { + if (!strcmp(argv[2],"-hil")) { + startup_in_hil = true; + } else { + PX4_ERR("Argument %s not supported.", argv[2]); + PX4_ERR("COMMANDER NOT STARTED"); + thread_should_exit = true; + } + } + /* set parameters */ param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); - param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); @@ -836,16 +939,22 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_autostart_id = param_find("SYS_AUTOSTART"); param_t _param_autosave_params = param_find("COM_AUTOS_PAR"); param_t _param_rc_in_off = param_find("COM_RC_IN_MODE"); - - const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; - main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; - main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL"; - main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL"; - main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; - main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; - main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; - main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; - main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; + param_t _param_eph = param_find("COM_HOME_H_T"); + param_t _param_epv = param_find("COM_HOME_V_T"); + param_t _param_geofence_action = param_find("GF_ACTION"); + param_t _param_disarm_land = param_find("COM_DISARM_LAND"); + param_t _param_map_mode_sw = param_find("RC_MAP_MODE_SW"); + + // const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; + // main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; + // main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL"; + // main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL"; + // main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + // main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + // main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + // main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; + // main_states_str[vehicle_status_s::MAIN_STATE_STAB] = "STAB"; + // main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; @@ -858,6 +967,8 @@ int commander_thread_main(int argc, char *argv[]) const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; @@ -888,8 +999,7 @@ int commander_thread_main(int argc, char *argv[]) if (battery_init() != OK) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: BATTERY INIT FAIL"); } - - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); /* vehicle status topic */ memset(&status, 0, sizeof(status)); @@ -900,7 +1010,12 @@ int commander_thread_main(int argc, char *argv[]) status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; - status.hil_state = vehicle_status_s::HIL_STATE_OFF; + + if(startup_in_hil) { + status.hil_state = vehicle_status_s::HIL_STATE_ON; + } else { + status.hil_state = vehicle_status_s::HIL_STATE_OFF; + } status.failsafe = false; /* neither manual nor offboard control commands have been received */ @@ -918,6 +1033,9 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; + + status.condition_system_prearm_error_reported = false; + status.condition_system_hotplug_timeout = false; status.counter++; status.timestamp = hrt_absolute_time(); @@ -942,21 +1060,18 @@ int commander_thread_main(int argc, char *argv[]) px4_task_exit(ERROR); } - /* armed topic */ - orb_advert_t armed_pub; /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); + /* armed topic */ + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); /* vehicle control mode topic */ memset(&control_mode, 0, sizeof(control_mode)); orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); - armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - /* home position */ orb_advert_t home_pub = nullptr; - struct home_position_s home; - memset(&home, 0, sizeof(home)); + memset(&_home, 0, sizeof(_home)); /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ orb_advert_t mission_pub = nullptr; @@ -1002,7 +1117,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - rc_calibration_check(mavlink_fd); + rc_calibration_check(mavlink_fd, true); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -1030,12 +1145,12 @@ int commander_thread_main(int argc, char *argv[]) memset(&offboard_control_mode, 0, sizeof(offboard_control_mode)); /* Subscribe to telemetry status topics */ - int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; - uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; - uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; - bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; + int telemetry_subs[ORB_MULTI_MAX_INSTANCES]; + uint64_t telemetry_last_heartbeat[ORB_MULTI_MAX_INSTANCES]; + uint64_t telemetry_last_dl_loss[ORB_MULTI_MAX_INSTANCES]; + bool telemetry_lost[ORB_MULTI_MAX_INSTANCES]; - for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { telemetry_subs[i] = -1; telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; @@ -1052,13 +1167,15 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - struct vehicle_local_position_s local_position; - memset(&local_position, 0, sizeof(local_position)); + struct vehicle_local_position_s local_position = {}; + + /* Subscribe to attitude data */ + int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + struct vehicle_attitude_s attitude = {}; /* Subscribe to land detector */ int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - struct vehicle_land_detected_s land_detector; - memset(&land_detector, 0, sizeof(land_detector)); + struct vehicle_land_detected_s land_detector = {}; /* * The home position is set based on GPS only, to prevent a dependency between @@ -1118,7 +1235,7 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to vtol vehicle status topic */ int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); - struct vtol_vehicle_status_s vtol_status; + //struct vtol_vehicle_status_s vtol_status; memset(&vtol_status, 0, sizeof(vtol_status)); vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing @@ -1141,24 +1258,33 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); - if (!status.condition_system_sensors_initialized) { - set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune - } - else { + int32_t rc_in_off = 0; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + param_get(_param_autostart_id, &autostart_id); + param_get(_param_rc_in_off, &rc_in_off); + status.rc_input_mode = rc_in_off; + if (is_hil_setup(autostart_id)) { + // HIL configuration selected: real sensors will be disabled + status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune + } else { + // sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, + checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false); + set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } - const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); + commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; - int32_t rc_in_off = 0; int32_t datalink_regain_timeout = 0; + int32_t geofence_action = 0; + /* Thresholds for engine failure detection */ int32_t ef_throttle_thres = 1.0f; int32_t ef_current2throttle_thres = 0.0f; @@ -1167,6 +1293,9 @@ int commander_thread_main(int argc, char *argv[]) int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */ + int32_t disarm_when_landed = 0; + int32_t map_mode_sw = 0; + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1175,7 +1304,7 @@ int commander_thread_main(int argc, char *argv[]) /* initialize low priority thread */ pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2600); + pthread_attr_setstacksize(&commander_low_prio_attr, 2880); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); @@ -1190,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { /* try to open the mavlink log device every once in a while */ - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); } arming_ret = TRANSITION_NOT_CHANGED; @@ -1231,15 +1360,21 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; + // check if main mode switch has been assigned and if so run preflight checks in order + // to update status.condition_sensors_initialised + int32_t map_mode_sw_new; + param_get(_param_map_mode_sw, &map_mode_sw_new); + + if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) { + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + } + /* re-check RC calibration */ - rc_calibration_check(mavlink_fd); + rc_calibration_check(mavlink_fd, true); } - /* navigation parameters */ - param_get(_param_takeoff_alt, &takeoff_alt); - /* Safety parameters */ - param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); @@ -1249,12 +1384,19 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); + param_get(_param_geofence_action, &geofence_action); + param_get(_param_disarm_land, &disarm_when_landed); + param_get(_param_map_mode_sw, &map_mode_sw); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); /* Parameter autosave setting */ param_get(_param_autosave_params, &autosave_params); + + /* EPH / EPV */ + param_get(_param_eph, &eph_threshold); + param_get(_param_epv, &epv_threshold); } /* Set flag to autosave parameters if necessary */ @@ -1289,10 +1431,10 @@ int commander_thread_main(int argc, char *argv[]) } } - for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (telemetry_subs[i] < 0 && (OK == orb_exists(telemetry_status_orb_id[i], 0))) { - telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + if (telemetry_subs[i] < 0 && (OK == orb_exists(ORB_ID(telemetry_status), i))) { + telemetry_subs[i] = orb_subscribe_multi(ORB_ID(telemetry_status), i); } orb_check(telemetry_subs[i], &updated); @@ -1301,15 +1443,24 @@ int commander_thread_main(int argc, char *argv[]) struct telemetry_status_s telemetry; memset(&telemetry, 0, sizeof(telemetry)); - orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry); + orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry); /* perform system checks when new telemetry link connected */ - if (mavlink_fd && - telemetry_last_heartbeat[i] == 0 && - telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { + if ( + /* we actually have a way to talk to the user */ + mavlink_fd && + /* we first connect a link or re-connect a link after loosing it */ + (telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&telemetry_last_heartbeat[i]) > 3 * 1000 * 1000)) && + /* and this link has a communication partner */ + (telemetry.heartbeat_time > 0) && + /* and it is still connected */ + (hrt_elapsed_time(&telemetry.heartbeat_time) < 2 * 1000 * 1000) && + /* and the system is not already armed (and potentially flying) */ + !armed.armed) { bool chAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { @@ -1317,8 +1468,20 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + if (is_hil_setup(autostart_id)) { + /* HIL configuration: check only RC input */ + (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true); + } else { + /* check sensors also */ + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); + } + } + + /* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */ + if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) { + _usb_telemetry_active = true; } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -1335,7 +1498,7 @@ int commander_thread_main(int argc, char *argv[]) * vertical separation from other airtraffic the operator has to know when the * barometer is inoperational. * */ - if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { + if (hrt_elapsed_time(&sensors.baro_timestamp[0]) < FAILSAFE_DEFAULT_TIMEOUT) { /* handle the case where baro was regained */ if (status.barometer_failure) { status.barometer_failure = false; @@ -1376,7 +1539,20 @@ int commander_thread_main(int argc, char *argv[]) /* copy avionics voltage */ status.avionics_power_rail_voltage = system_power.voltage5V_v; - status.usb_connected = system_power.usb_connected; + + /* if the USB hardware connection went away, reboot */ + if (status.usb_connected && !system_power.usb_connected) { + /* + * apparently the USB cable went away but we are still powered, + * so lets reset to a classic non-usb state. + */ + mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.") + usleep(400000); + px4_systemreset(false); + } + + /* finally judge the USB connected state based on software detection */ + status.usb_connected = _usb_telemetry_active; } } @@ -1426,6 +1602,7 @@ int commander_thread_main(int argc, char *argv[]) /* Make sure that this is only adjusted if vehicle really is of type vtol*/ if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; + status.in_transition_mode = vtol_status.vtol_in_trans_mode; } } @@ -1434,7 +1611,22 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { /* position changed */ - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + vehicle_global_position_s gpos; + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos); + + /* copy to global struct if valid, with hysteresis */ + + // XXX consolidate this with local position handling and timeouts after release + // but we want a low-risk change now. + if (status.condition_global_position_valid) { + if (gpos.eph < eph_threshold * 2.5f) { + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + } + } else { + if (gpos.eph < eph_threshold) { + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + } + } } /* update local position estimate */ @@ -1445,19 +1637,26 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } + /* update attitude estimate */ + orb_check(attitude_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude); + } + //update condition_global_position_valid //Global positions are only published by the estimators if they are valid - if(hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { + if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { //We have had no good fix for POSITION_TIMEOUT amount of time - if(status.condition_global_position_valid) { + if (status.condition_global_position_valid) { set_tune_override(TONE_GPS_WARNING_TUNE); status_changed = true; status.condition_global_position_valid = false; } - } - else if(global_position.timestamp != 0) { - //Got good global position estimate - if(!status.condition_global_position_valid) { + } else if (global_position.timestamp != 0) { + // Got good global position estimate + if (!status.condition_global_position_valid) { status_changed = true; status.condition_global_position_valid = true; } @@ -1490,21 +1689,40 @@ int commander_thread_main(int argc, char *argv[]) &(status.condition_local_altitude_valid), &status_changed); /* Update land detector */ + static bool check_for_disarming = false; orb_check(land_detector_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); } - if (updated && status.condition_local_altitude_valid) { + if ((updated && status.condition_local_altitude_valid) || check_for_disarming) { if (status.condition_landed != land_detector.landed) { status.condition_landed = land_detector.landed; status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "LANDING DETECTED"); + mavlink_and_console_log_info(mavlink_fd, "LANDING DETECTED"); } else { - mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED"); + mavlink_and_console_log_info(mavlink_fd, "TAKEOFF DETECTED"); + } + } + + if (disarm_when_landed > 0) { + if (land_detector.landed) { + if (!check_for_disarming && _inair_last_time > 0) { + _inair_last_time = land_detector.timestamp; + check_for_disarming = true; + } + + if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) { + mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING"); + arm_disarm(false, mavlink_fd, "auto disarm on land"); + _inair_last_time = 0; + check_for_disarming = false; + } + } else { + _inair_last_time = land_detector.timestamp; } } } @@ -1589,6 +1807,8 @@ int commander_thread_main(int argc, char *argv[]) low_battery_voltage_actions_done = true; if (armed.armed) { mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); + } else { + mavlink_log_critical(mavlink_fd, "LOW BATTERY, TAKEOFF DISCOURAGED"); } status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; @@ -1625,6 +1845,9 @@ int commander_thread_main(int argc, char *argv[]) if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; + } else if (arming_ret == TRANSITION_DENIED) { + /* do not complain if not allowed into standby */ + arming_ret = TRANSITION_NOT_CHANGED; } } @@ -1676,7 +1899,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.gps_failure && !gpsIsNoisy) { status.gps_failure = false; status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); + mavlink_log_critical(mavlink_fd, "gps fix regained"); } } else if (!status.gps_failure) { @@ -1700,37 +1923,144 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result); } - /* Check for geofence violation */ - if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) { - //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ + // Geofence actions + if (armed.armed && (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)) { + + static bool geofence_loiter_on = false; + static bool geofence_rtl_on = false; + + static uint8_t geofence_main_state_before_violation = vehicle_status_s::MAIN_STATE_MAX; + + // check for geofence violation + if (geofence_result.geofence_violated) { + static hrt_abstime last_geofence_violation = 0; + const hrt_abstime geofence_violation_action_interval = 10000000; // 10 seconds + if (hrt_elapsed_time(&last_geofence_violation) > geofence_violation_action_interval) { + + last_geofence_violation = hrt_absolute_time(); + + switch (geofence_result.geofence_action) { + case (geofence_result_s::GF_ACTION_NONE) : { + // do nothing + break; + } + case (geofence_result_s::GF_ACTION_WARN) : { + // do nothing, mavlink critical messages are sent by navigator + break; + } + case (geofence_result_s::GF_ACTION_LOITER) : { + if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER)) { + geofence_loiter_on = true; + } + break; + } + case (geofence_result_s::GF_ACTION_RTL) : { + if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL)) { + geofence_rtl_on = true; + } + break; + } + case (geofence_result_s::GF_ACTION_TERMINATE) : { + warnx("Flight termination because of geofence"); + mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination"); + armed.force_failsafe = true; + status_changed = true; + break; + } + } + } + } + + // reset if no longer in LOITER or if manually switched to LOITER + geofence_loiter_on = geofence_loiter_on + && (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LOITER) + && (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF); + + // reset if no longer in RTL or if manually switched to RTL + geofence_rtl_on = geofence_rtl_on + && (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_RTL) + && (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF); + + if (!geofence_loiter_on && !geofence_rtl_on) { + // store the last good main_state when not in a geofence triggered action (LOITER or RTL) + geofence_main_state_before_violation = status.main_state; + } + + // revert geofence failsafe transition if sticks are moved and we were previously in MANUAL or ASSIST + if ((geofence_loiter_on || geofence_rtl_on) && + (geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_MANUAL || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ALTCTL || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_POSCTL || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ACRO || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_STAB)) { + + // transition to previous state if sticks are increased + const float min_stick_change = 0.2; + if ((_last_sp_man.timestamp != sp_man.timestamp) && + ((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) || + (fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) || + (fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) || + (fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) { + + main_state_transition(&status, geofence_main_state_before_violation); + } + } + } + + + /* Check for mission flight termination */ + if (armed.armed && mission_result.flight_termination) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; if (!flight_termination_printed) { - warnx("Flight termination because of navigator request or geofence"); - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + warnx("Flight termination because of navigator request"); flight_termination_printed = true; } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + mavlink_log_critical(mavlink_fd, "Flight termination active"); + } + } + + /* Only evaluate mission state if home is set, + * this prevents false positives for the mission + * rejection. Back off 2 seconds to not overlay + * home tune. + */ + if (status.condition_home_position_valid && + (hrt_elapsed_time(&_home.timestamp) > 2000000) && + _last_mission_instance != mission_result.instance_count) { + if (!mission_result.valid) { + /* the mission is invalid */ + tune_mission_fail(true); + warnx("mission fail"); + } else if (mission_result.warning) { + /* the mission has a warning */ + tune_mission_fail(true); + warnx("mission warning"); + } else { + /* the mission is valid */ + tune_mission_ok(true); + warnx("mission ok"); } - } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination + + /* prevent further feedback until the mission changes */ + _last_mission_instance = mission_result.instance_count; + } /* RC input check */ - if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && - hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && + (hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_info(mavlink_fd, "Detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums", + mavlink_log_info(mavlink_fd, "MANUAL CONTROL REGAINED after %llums", (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000); status_changed = true; } @@ -1738,11 +2068,15 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm + /* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ - if (status.is_rotary_wing && + if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && - (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) && + (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state == vehicle_status_s::MAIN_STATE_ACRO || + status.main_state == vehicle_status_s::MAIN_STATE_STAB || + status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE || + status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1767,23 +2101,30 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY && - sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { + if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF ) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* we check outside of the transition function here because the requirement * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ - if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) { + + if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) && + (status.main_state != vehicle_status_s::MAIN_STATE_STAB)) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); - } else { + } else if (!status.condition_home_position_valid && + geofence_action == geofence_result_s::GF_ACTION_RTL) { + print_reject_arm("NOT ARMING: Geofence RTL requires valid home"); + + } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; + } else { + print_reject_arm("NOT ARMING: Configuration error"); } } @@ -1831,8 +2172,8 @@ int commander_thread_main(int argc, char *argv[]) } } else { - if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)", hrt_absolute_time() / 1000); + if (!status.rc_input_blocked && !status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000); status.rc_signal_lost = true; status.rc_signal_lost_timestamp = sp_man.timestamp; status_changed = true; @@ -1842,7 +2183,7 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; - for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { /* handle the case where data link was gained first time or regained, @@ -1851,11 +2192,17 @@ int commander_thread_main(int argc, char *argv[]) if (telemetry_lost[i] && hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { - /* only report a regain */ + /* report a regain */ if (telemetry_last_dl_loss[i] > 0) { - mavlink_and_console_log_critical(mavlink_fd, "data link #%i regained", i); + mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); + } else if (telemetry_last_dl_loss[i] == 0) { + /* new link */ } + /* got link again or new */ + status.condition_system_prearm_error_reported = false; + status_changed = true; + telemetry_lost[i] = false; have_link = true; @@ -1871,7 +2218,7 @@ int commander_thread_main(int argc, char *argv[]) /* only reset the timestamp to a different time on state change */ telemetry_last_dl_loss[i] = hrt_absolute_time(); - mavlink_and_console_log_critical(mavlink_fd, "data link #%i lost", i); + mavlink_and_console_log_info(mavlink_fd, "data link #%i lost", i); telemetry_lost[i] = true; } } @@ -1886,7 +2233,9 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { - mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); + if (armed.armed) { + mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); + } status.data_link_lost = true; status.data_link_lost_counter++; status_changed = true; @@ -1932,7 +2281,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &attitude, &home_pub)) { status_changed = true; } } @@ -1940,10 +2289,12 @@ int commander_thread_main(int argc, char *argv[]) /* Check for failure combinations which lead to flight termination */ if (armed.armed) { /* At this point the data link and the gps system have been checked - * If we are not in a manual (RC stick controlled mode) + * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && + status.main_state !=vehicle_status_s::MAIN_STATE_RATTITUDE && + status.main_state !=vehicle_status_s::MAIN_STATE_STAB && status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && ((status.data_link_lost && status.gps_failure) || @@ -1967,7 +2318,9 @@ int commander_thread_main(int argc, char *argv[]) * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || + status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE || status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state !=vehicle_status_s::MAIN_STATE_STAB || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status.gps_failure) || @@ -1987,19 +2340,23 @@ int commander_thread_main(int argc, char *argv[]) } } - //Get current timestamp + /* Get current timestamp */ const hrt_abstime now = hrt_absolute_time(); - //First time home position update - if (!status.condition_home_position_valid) { - commander_set_home_position(home_pub, home, local_position, global_position); + /* First time home position update - but only if disarmed */ + if (!status.condition_home_position_valid && !armed.armed) { + commander_set_home_position(home_pub, _home, local_position, global_position, attitude); } - /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { - commander_set_home_position(home_pub, home, local_position, global_position); + /* update home position on arming if at least 1s from commander start spent to avoid setting home on in-air restart */ + else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) && + (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { + commander_set_home_position(home_pub, _home, local_position, global_position, attitude); } + was_landed = status.condition_landed; + was_armed = armed.armed; + /* print new state */ if (arming_state_changed) { status_changed = true; @@ -2007,21 +2364,11 @@ int commander_thread_main(int argc, char *argv[]) arming_state_changed = false; } - was_armed = armed.armed; - /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, mission_result.finished, mission_result.stay_in_failsafe); - // TODO handle mode changes by commands - if (main_state_changed) { - status_changed = true; - warnx("main state: %s", main_states_str[status.main_state]); - mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); - main_state_changed = false; - } - if (status.failsafe != failsafe_old) { status_changed = true; @@ -2035,10 +2382,11 @@ int commander_thread_main(int argc, char *argv[]) failsafe_old = status.failsafe; } - if (nav_state_changed) { + // TODO handle mode changes by commands + if (main_state_changed || nav_state_changed) { status_changed = true; - warnx("nav state: %s", nav_states_str[status.nav_state]); - mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]); + mavlink_log_info(mavlink_fd, "Flight mode: %s", nav_states_str[status.nav_state]); + main_state_changed = false; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ @@ -2051,6 +2399,19 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_status), status_pub, &status); armed.timestamp = now; + + /* set prearmed state if safety is off, or safety is not present and 5 seconds passed */ + if (safety.safety_switch_available) { + + /* safety is off, go into prearmed */ + armed.prearmed = safety.safety_off; + } else { + /* safety is not present, go into prearmed + * (all output drivers should be started / unlocked last in the boot process + * when the rest of the system is fully initialized) + */ + armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000); + } orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -2061,18 +2422,20 @@ int commander_thread_main(int argc, char *argv[]) set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; - } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { + } else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) && + status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { + } else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) && + (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe)) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); } else { set_tune(TONE_STOP_TUNE); } - + /* reset arm_tune_played when disarmed */ if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { @@ -2083,8 +2446,22 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } + + /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ + hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + + if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) { + set_tune_override(TONE_GPS_WARNING_TUNE); + sensor_fail_tune_played = true; + status_changed = true; + } + + /* update timeout flag */ + if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) { + status.condition_system_hotplug_timeout = hotplug_timeout; + status_changed = true; + } - fflush(stdout); counter++; int blink_state = blink_msg_state(); @@ -2118,18 +2495,18 @@ int commander_thread_main(int argc, char *argv[]) /* close fds */ led_deinit(); buzzer_deinit(); - close(sp_man_sub); - close(offboard_control_mode_sub); - close(local_position_sub); - close(global_position_sub); - close(gps_sub); - close(sensor_sub); - close(safety_sub); - close(cmd_sub); - close(subsys_sub); - close(diff_pres_sub); - close(param_changed_sub); - close(battery_sub); + px4_close(sp_man_sub); + px4_close(offboard_control_mode_sub); + px4_close(local_position_sub); + px4_close(global_position_sub); + px4_close(gps_sub); + px4_close(sensor_sub); + px4_close(safety_sub); + px4_close(cmd_sub); + px4_close(subsys_sub); + px4_close(diff_pres_sub); + px4_close(param_changed_sub); + px4_close(battery_sub); thread_running = false; @@ -2141,6 +2518,8 @@ get_circuit_breaker_params() { status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.cb_usb = + circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY); status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status.circuit_breaker_engaged_enginefailure_check = @@ -2167,19 +2546,24 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* driving rgbled */ if (changed) { bool set_normal_color = false; - + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* set mode */ if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status.condition_system_sensors_initialized && hotplug_timeout)) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; + + } else if (!status.condition_system_sensors_initialized && !hotplug_timeout) { + rgbled_set_mode(RGBLED_MODE_BREATHE); + set_normal_color = true; } else { // STANDBY_ERROR and other states rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); @@ -2195,7 +2579,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { rgbled_set_color(RGBLED_COLOR_RED); } else { - if (status_local->condition_global_position_valid) { + if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { @@ -2246,11 +2630,34 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; - /* if offboard is set allready by a mavlink command, abort */ + /* if offboard is set already by a mavlink command, abort */ if (status.offboard_control_set_by_command) { return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); } + /* manual setpoint has not updated, do not re-evaluate it */ + if ((_last_sp_man.timestamp == sp_man->timestamp) || + ((_last_sp_man.offboard_switch == sp_man->offboard_switch) && + (_last_sp_man.return_switch == sp_man->return_switch) && + (_last_sp_man.mode_switch == sp_man->mode_switch) && + (_last_sp_man.acro_switch == sp_man->acro_switch) && + (_last_sp_man.rattitude_switch == sp_man->rattitude_switch) && + (_last_sp_man.posctl_switch == sp_man->posctl_switch) && + (_last_sp_man.loiter_switch == sp_man->loiter_switch))) { + + // update these fields for the geofence system + _last_sp_man.timestamp = sp_man->timestamp; + _last_sp_man.x = sp_man->x; + _last_sp_man.y = sp_man->y; + _last_sp_man.z = sp_man->z; + _last_sp_man.r = sp_man->r; + + /* no timestamp change or no switch change -> nothing changed */ + return TRANSITION_NOT_CHANGED; + } + + _last_sp_man = *sp_man; + /* offboard switch overrides main switch */ if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); @@ -2292,9 +2699,26 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); - } else { + /* manual mode is stabilized already for multirotors, so switch to acro + * for any non-manual mode + */ + if (status.is_rotary_wing) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); + } else { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); + } + + } + else if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){ + /* Similar to acro transitions for multirotors. FW aircraft don't need a + * rattitude mode.*/ + if (status.is_rotary_wing) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE); + } else { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); + } + }else { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); } @@ -2394,8 +2818,34 @@ set_control_mode() case vehicle_status_s::NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); - control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); + control_mode.flag_control_rates_enabled = stabilization_required(); + control_mode.flag_control_attitude_enabled = stabilization_required(); + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + + case vehicle_status_s::NAVIGATION_STATE_STAB: + 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 = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + /* override is not ok in stabilized mode */ + control_mode.flag_external_manual_override_ok = false; + break; + + case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: + 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 = false; control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; @@ -2422,17 +2872,20 @@ set_control_mode() 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_position_enabled = !status.in_transition_mode; + control_mode.flag_control_velocity_enabled = !status.in_transition_mode; control_mode.flag_control_termination_enabled = false; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: + /* override is not ok for the RTL and recovery mode */ + control_mode.flag_external_manual_override_ok = false; + /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2528,15 +2981,16 @@ set_control_mode() !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_acceleration_force; - control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position; + control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position) && !status.in_transition_mode; control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position; - control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position; + control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode; - control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; + control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position; break; @@ -2545,6 +2999,15 @@ set_control_mode() } } +bool +stabilization_required() +{ + return (status.is_rotary_wing || // is a rotary wing, or + status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or + (vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND + !status.is_rotary_wing)); // is a fixed wing, ie: transitioning back to rotary wing mode +} + void print_reject_mode(struct vehicle_status_s *status_local, const char *msg) { @@ -2607,7 +3070,7 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result) void *commander_low_prio_loop(void *arg) { -#ifndef __PX4_QURT +#if defined(__PX4_LINUX) || defined(__PX4_NUTTX) /* Set thread name */ prctl(PR_SET_NAME, "commander_low_prio", getpid()); #endif @@ -2638,11 +3101,10 @@ void *commander_low_prio_loop(void *arg) if (need_param_autosave_timeout > 0 && hrt_elapsed_time(&need_param_autosave_timeout) > 200000ULL) { int ret = param_save_default(); - if (ret == OK) { - mavlink_and_console_log_info(mavlink_fd, "settings autosaved"); - + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, "settings auto save error"); } else { - mavlink_and_console_log_critical(mavlink_fd, "settings save error"); + warnx("settings saved."); } need_param_autosave = false; @@ -2778,6 +3240,7 @@ void *commander_low_prio_loop(void *arg) // so this would be prone to false negatives. bool checkAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { @@ -2785,7 +3248,7 @@ void *commander_low_prio_loop(void *arg) } status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); @@ -2845,6 +3308,21 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); } + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + } + } else if (((int)(cmd.param1)) == 2) { + + /* reset parameters and save empty file */ + param_reset_all(); + int ret = param_save_default(); + + if (ret == OK) { + /* do not spam MAVLink, but provide the answer / green led mechanism */ + mavlink_log_critical(mavlink_fd, "onboard parameters reset"); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + mavlink_log_critical(mavlink_fd, "param reset error"); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); } } @@ -2870,7 +3348,7 @@ void *commander_low_prio_loop(void *arg) } } - close(cmd_sub); + px4_close(cmd_sub); return NULL; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index b902952ae25e..f411e1c128dc 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -110,6 +110,7 @@ static float bat_v_load_drop = 0.06f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static unsigned int counter = 0; +static float throttle_lowpassed = 0.0f; int battery_init() { @@ -174,6 +175,39 @@ void set_tune(int tune) } } +void tune_home_set(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_HOME_SET); + } +} + +void tune_mission_ok(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEUTRAL_TUNE); + } +} + +void tune_mission_fail(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEGATIVE_TUNE); + } +} + /** * Blink green LED and play positive tune (if use_buzzer == true). */ @@ -350,9 +384,20 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; + // XXX this time constant needs to become tunable + // but really, the right fix are smart batteries. + float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f; + if (PX4_ISFINITE(val)) { + throttle_lowpassed = val; + } + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized); - float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty)); + float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_lowpassed); + /* the range from full to empty is the same for batteries under load and without load, + * since the voltage drop applies to both the full and empty state + */ + float voltage_range = (bat_v_full - bat_v_empty); + float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty_dynamic)) / (bat_n_cells * voltage_range); if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index d2aace2a407a..d2ab41f8875f 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -58,6 +58,9 @@ void buzzer_deinit(void); void set_tune_override(int tune); void set_tune(int tune); +void tune_home_set(bool use_buzzer); +void tune_mission_ok(bool use_buzzer); +void tune_mission_fail(bool use_buzzer); void tune_positive(bool use_buzzer); void tune_neutral(bool use_buzzer); void tune_negative(bool use_buzzer); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 90cc1d91b8f7..4b06ecbdf464 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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 @@ -37,16 +36,57 @@ * * Parameters defined by the sensors task. * - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes */ #include #include +/** + * Roll trim + * + * The trim value is the actuator control value the system needs + * for straight and level flight. It can be calibrated by + * flying manually straight and level using the RC trims and + * copying them using the GCS. + * + * @group Radio Calibration + * @min -0.25 + * @max 0.25 + * @decimal 2 + */ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); + +/** + * Pitch trim + * + * The trim value is the actuator control value the system needs + * for straight and level flight. It can be calibrated by + * flying manually straight and level using the RC trims and + * copying them using the GCS. + * + * @group Radio Calibration + * @min -0.25 + * @max 0.25 + * @decimal 2 + */ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); + +/** + * Yaw trim + * + * The trim value is the actuator control value the system needs + * for straight and level flight. It can be calibrated by + * flying manually straight and level using the RC trims and + * copying them using the GCS. + * + * @group Radio Calibration + * @min -0.25 + * @max 0.25 + * @decimal 2 + */ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); /** @@ -56,6 +96,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * * @group Battery Calibration * @unit V + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); @@ -66,6 +107,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration * @unit V + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); @@ -77,6 +119,9 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * * @group Battery Calibration * @unit V + * @min 0.0 + * @max 1.5 + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); @@ -87,6 +132,8 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); * * @group Battery Calibration * @unit S + * @min 1 + * @max 10 */ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); @@ -97,6 +144,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * * @group Battery Calibration * @unit mA + * @decimal 0 */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); @@ -144,17 +192,20 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * @group Commander * @min 0.0 * @max 1.0 + * @decimal 1 */ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); /** * Engine Failure Current/Throttle Threshold * - * Engine failure triggers only below this current/throttle value + * Engine failure triggers only below this current value * * @group Commander * @min 0.0 - * @max 7.0 + * @max 30.0 + * @unit ampere + * @decimal 2 */ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); @@ -167,7 +218,8 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @group Commander * @unit second * @min 0.0 - * @max 7.0 + * @max 60.0 + * @decimal 1 */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); @@ -180,8 +232,35 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); * @unit second * @min 0 * @max 35 + * @decimal 1 */ -PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); +PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); + +/** + * Home set horizontal threshold + * + * The home position will be set if the estimated positioning accuracy is below the threshold. + * + * @group Commander + * @unit meter + * @min 2 + * @max 15 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f); + +/** + * Home set vertical threshold + * + * The home position will be set if the estimated positioning accuracy is below the threshold. + * + * @group Commander + * @unit meter + * @min 5 + * @max 25 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f); /** * Autosaving of params @@ -209,3 +288,16 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1); * @max 2 */ PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0); + +/** + * Time-out for auto disarm after landing + * + * A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be + * automatically disarmed in case a landing situation has been detected during this period. + * A value of zero means that automatic disarming is disabled. + * + * @group Commander + * @min 0 + */ +PARAM_DEFINE_INT32(COM_DISARM_LAND, 0); + diff --git a/src/modules/commander/commander_tests/CMakeLists.txt b/src/modules/commander/commander_tests/CMakeLists.txt new file mode 100644 index 000000000000..1a604136adcd --- /dev/null +++ b/src/modules/commander/commander_tests/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__commander__commander_tests + MAIN commander_tests + SRCS + commander_tests.cpp + state_machine_helper_test.cpp + ../state_machine_helper.cpp + ../PreflightCheck.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 817cbcdb0ede..1fcd48b088ef 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -122,7 +122,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { - mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); } } @@ -131,14 +131,14 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } if (poll_errcount > 1000) { - mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); + mavlink_and_console_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); return calibrate_return_error; } } for (unsigned s = 0; s < max_gyros; s++) { if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) { - mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) + mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) return calibrate_return_error; } @@ -155,7 +155,7 @@ int do_gyro_calibration(int mavlink_fd) int res = OK; gyro_worker_data_t worker_data = {}; - mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); worker_data.mavlink_fd = mavlink_fd; @@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, // z scale }; + int device_prio_max = 0; + int32_t device_id_primary = 0; + for (unsigned s = 0; s < max_gyros; s++) { char str[30]; @@ -176,7 +179,7 @@ int do_gyro_calibration(int mavlink_fd) (void)sprintf(str, "CAL_GYRO%u_ID", s); res = param_set_no_notification(param_find(str), &(worker_data.device_id[s])); if (res != OK) { - mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); return ERROR; } @@ -190,7 +193,7 @@ int do_gyro_calibration(int mavlink_fd) px4_close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); return ERROR; } } @@ -199,6 +202,15 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); + + // Get priority + int32_t prio; + orb_priority(worker_data.gyro_sensor_sub[s], &prio); + + if (prio > device_prio_max) { + device_prio_max = prio; + device_id_primary = worker_data.device_id[s]; + } } int cancel_sub = calibrate_cancel_subscribe(); @@ -226,7 +238,7 @@ int do_gyro_calibration(int mavlink_fd) float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset; /* maximum allowable calibration error in radians */ - const float maxoff = 0.0055f; + const float maxoff = 0.01f; if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) || !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) || @@ -258,9 +270,12 @@ int do_gyro_calibration(int mavlink_fd) } if (res == OK) { + /* set offset parameters to new values */ bool failed = false; + failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary))); + for (unsigned s = 0; s < max_gyros; s++) { if (worker_data.device_id[s] != 0) { char str[30]; @@ -287,7 +302,7 @@ int do_gyro_calibration(int mavlink_fd) px4_close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1); } } } @@ -310,7 +325,7 @@ int do_gyro_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); } } @@ -318,9 +333,9 @@ int do_gyro_calibration(int mavlink_fd) usleep(200000); if (res == OK) { - mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); } else { - mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } /* give this message enough time to propagate */ diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 10d32b0995f3..eba8feec99df 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -67,7 +67,15 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; -static const unsigned max_mags = 3; +static constexpr unsigned max_mags = 3; +static constexpr float mag_sphere_radius = 0.2f; +static constexpr unsigned int calibration_sides = 6; ///< The total number of sides +static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer +static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take + +int32_t device_ids[max_mags]; +int device_prio_max = 0; +int32_t device_id_primary = 0; calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); @@ -79,7 +87,7 @@ typedef struct { unsigned int calibration_points_perside; unsigned int calibration_interval_perside_seconds; uint64_t calibration_interval_perside_useconds; - unsigned int calibration_counter_total; + unsigned int calibration_counter_total[max_mags]; bool side_data_collected[detect_orientation_side_count]; float* x[max_mags]; float* y[max_mags]; @@ -104,7 +112,6 @@ int do_mag_calibration(int mavlink_fd) // Determine which mags are available and reset each - int32_t device_ids[max_mags]; char str[30]; for (size_t i=0; idone_count) / calibration_sides; +} + static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { calibrate_return result = calibrate_return_ok; @@ -206,7 +235,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta * for a good result, so we're not constraining the user more than we have to. */ - hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; + hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 5; hrt_abstime last_gyro = 0; float gyro_x_integral = 0.0f; float gyro_y_integral = 0.0f; @@ -223,7 +252,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta /* abort on request */ if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { result = calibrate_return_cancelled; - close(sub_gyro); + px4_close(sub_gyro); return result; } @@ -236,12 +265,12 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } /* Wait clocking for new data on all gyro */ - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = sub_gyro; fds[0].events = POLLIN; size_t fd_count = 1; - int poll_ret = poll(fds, fd_count, 1000); + int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { struct gyro_report gyro; @@ -261,7 +290,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } } - close(sub_gyro); + px4_close(sub_gyro); uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; unsigned poll_errcount = 0; @@ -289,27 +318,47 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { + + int prev_count[max_mags]; + bool rejected = false; + for (size_t cur_mag=0; cur_magcalibration_counter_total[cur_mag]; + if (worker_data->sub_mag[cur_mag] >= 0) { struct mag_report mag; orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); + + // Check if this measurement is good to go in + rejected = rejected || reject_sample(mag.x, mag.y, mag.z, + worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], + worker_data->calibration_counter_total[cur_mag], + calibration_sides * worker_data->calibration_points_perside); - worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x; - worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y; - worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z; - + worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x; + worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y; + worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z; + worker_data->calibration_counter_total[cur_mag]++; } } - - worker_data->calibration_counter_total++; - calibration_counter_side++; - - // Progress indicator for side - mavlink_and_console_log_info(worker_data->mavlink_fd, - "[cal] %s side calibration: progress <%u>", - detect_orientation_str(orientation), - (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + + // Keep calibration of all mags in lockstep + if (rejected) { + // Reset counts, since one of the mags rejected the measurement + for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag]; + } + } else { + calibration_counter_side++; + + // Progress indicator for side + mavlink_and_console_log_info(worker_data->mavlink_fd, + "[cal] %s side calibration: progress <%u>", + detect_orientation_str(orientation), progress_percentage(worker_data) + + (unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + } } else { poll_errcount++; } @@ -325,7 +374,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation)); worker_data->done_count++; - mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count); + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data)); } return result; @@ -339,18 +388,17 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; - worker_data.calibration_counter_total = 0; - worker_data.calibration_points_perside = 80; - worker_data.calibration_interval_perside_seconds = 20; + worker_data.calibration_points_perside = calibration_total_points / calibration_sides; + worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; // Collect: Right-side up, Left Side, Nose down worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false; worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false; worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false; - worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true; - worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; - worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true; + worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false; for (size_t cur_mag=0; cur_mag device_prio_max) { + device_prio_max = prio; + device_id_primary = device_ids[cur_mag]; + } } } } @@ -441,7 +498,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag // Mag in this slot is available and we should have values for it to calibrate sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], - worker_data.calibration_counter_total, + worker_data.calibration_counter_total[cur_mag], 100, 0.0f, &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag], &sphere_radius[cur_mag]); @@ -453,6 +510,49 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag } } } + + // Print uncalibrated data points + if (result == calibrate_return_ok) { + + printf("RAW DATA:\n--------------------\n"); + for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + + if (worker_data.calibration_counter_total[cur_mag] == 0) { + continue; + } + + printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); + + for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { + float x = worker_data.x[cur_mag][i]; + float y = worker_data.y[cur_mag][i]; + float z = worker_data.z[cur_mag][i]; + printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); + } + + printf(">>>>>>>\n"); + } + + printf("CALIBRATED DATA:\n--------------------\n"); + for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + + if (worker_data.calibration_counter_total[cur_mag] == 0) { + continue; + } + + printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); + + for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { + float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag]; + float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag]; + float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag]; + printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); + } + + printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]); + printf(">>>>>>>\n"); + } + } // Data points are no longer needed for (size_t cur_mag=0; cur_mag */ #ifndef PX4_CUSTOM_MODE_H_ @@ -17,6 +50,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_MAIN_MODE_ACRO, PX4_CUSTOM_MAIN_MODE_OFFBOARD, + PX4_CUSTOM_MAIN_MODE_STABILIZED }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 8c51e95b9dcb..484087a2349a 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -59,7 +59,7 @@ static const int ERROR = -1; int do_trim_calibration(int mavlink_fd) { int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - usleep(200000); + usleep(400000); struct manual_control_setpoint_s sp; bool changed; orb_check(sub_man, &changed); @@ -71,20 +71,34 @@ int do_trim_calibration(int mavlink_fd) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - /* set parameters */ - float p = sp.y; - param_set(param_find("TRIM_ROLL"), &p); - p = sp.x; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.r; - param_set(param_find("TRIM_YAW"), &p); + /* load trim values which are active */ + float roll_trim_active; + param_get(param_find("TRIM_ROLL"), &roll_trim_active); + float pitch_trim_active; + param_get(param_find("TRIM_PITCH"), &pitch_trim_active); + float yaw_trim_active; + param_get(param_find("TRIM_YAW"), &yaw_trim_active); + + /* set parameters: the new trim values are the combination of active trim values + and the values coming from the remote control of the user + */ + float p = sp.y + roll_trim_active; + int p1r = param_set(param_find("TRIM_ROLL"), &p); + /* + we explicitly swap sign here because the trim is added to the actuator controls + which are moving in an inverse sense to manual pitch inputs + */ + p = -sp.x + pitch_trim_active; + int p2r = param_set(param_find("TRIM_PITCH"), &p); + p = sp.r + yaw_trim_active; + int p3r = param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ /* auto-save */ int save_ret = param_save_default(); - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL"); + if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) { + mavlink_log_critical(mavlink_fd, "TRIM: PARAM SET FAIL"); px4_close(sub_man); return ERROR; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index fdbcaa3256d7..478bad759511 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -39,7 +39,6 @@ * @author Julian Oes */ -#include #include #include #include @@ -49,6 +48,8 @@ #include #include +#include + #include #include #include @@ -65,12 +66,6 @@ #include "commander_helper.h" #include "PreflightCheck.h" -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even @@ -125,17 +120,28 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s */ int prearm_ret = OK; - /* only perform the check if we have to */ - if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - prearm_ret = prearm_check(status, mavlink_fd); + /* only perform the pre-arm check if we have to */ + if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + + prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ ); + } + /* re-run the pre-flight check as long as sensors are failing */ + if (!status->condition_system_sensors_initialized + && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || + new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + + prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); + status->condition_system_sensors_initialized = !prearm_ret; } -#ifdef __PX4_NUTTX /* * Perform an atomic state update */ + #ifdef __PX4_NUTTX irqstate_t flags = irqsave(); -#endif + #endif /* enforce lockdown in HIL */ if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { @@ -173,7 +179,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); feedback_provided = true; valid_transition = false; } @@ -198,10 +204,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s feedback_provided = true; valid_transition = false; } else if (status->avionics_power_rail_voltage < 4.9f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } else if (status->avionics_power_rail_voltage > 5.4f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } } @@ -245,12 +251,14 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { - if (!fRunPreArmChecks) { + if ((!status->condition_system_prearm_error_reported && + status->condition_system_hotplug_timeout) || + (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); + status->condition_system_prearm_error_reported = true; } feedback_provided = true; valid_transition = false; - status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } // Finish up the state transition @@ -261,20 +269,23 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s status->arming_state = new_arming_state; } -#ifdef __PX4_NUTTX + /* reset feedback state */ + if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR && + status->arming_state != vehicle_status_s::ARMING_STATE_INIT && + valid_transition) { + status->condition_system_prearm_error_reported = false; + } + /* end of atomic state update */ + #ifdef __PX4_NUTTX irqrestore(flags); -#endif + #endif } if (ret == TRANSITION_DENIED) { - const char * str = "INVAL: %s - %s"; - /* only print to console here by default as this is too technical to be useful during operation */ - warnx(str, state_names[status->arming_state], state_names[new_arming_state]); - - /* print to MAVLink if we didn't provide any feedback yet */ + /* print to MAVLink and console if we didn't provide any feedback yet */ if (!feedback_provided) { - mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]); + mavlink_and_console_log_critical(mavlink_fd, "INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]); } } @@ -304,6 +315,8 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta switch (new_main_state) { case vehicle_status_s::MAIN_STATE_MANUAL: case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_RATTITUDE: + case vehicle_status_s::MAIN_STATE_STAB: ret = TRANSITION_CHANGED; break; @@ -364,146 +377,135 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta return ret; } -#ifdef __PX4_NUTTX -static transition_result_t disable_publication(const int mavlink_fd) +/** + * Transition from one hil state to another + */ +transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - transition_result_t ret; + transition_result_t ret = TRANSITION_DENIED; - /* Disable publication of all attached sensors */ - /* list directory */ - DIR *d; - d = opendir("/dev"); + if (current_status->hil_state == new_state) { + ret = TRANSITION_NOT_CHANGED; - if (d) { - struct dirent *direntry; - char devname[24]; + } else { + switch (new_state) { + case vehicle_status_s::HIL_STATE_OFF: + /* we're in HIL and unexpected things can happen if we disable HIL now */ + mavlink_and_console_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + ret = TRANSITION_DENIED; + break; - while ((direntry = readdir(d)) != NULL) { + case vehicle_status_s::HIL_STATE_ON: + if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - /* skip serial ports */ - if (!strncmp("tty", direntry->d_name, 3)) { - continue; - } +#ifdef __PX4_NUTTX + /* Disable publication of all attached sensors */ + /* list directory */ + DIR *d; + d = opendir("/dev"); - /* skip mtd devices */ - if (!strncmp("mtd", direntry->d_name, 3)) { - continue; - } + if (d) { + struct dirent *direntry; + char devname[24]; - /* skip ram devices */ - if (!strncmp("ram", direntry->d_name, 3)) { - continue; - } + while ((direntry = readdir(d)) != NULL) { - /* skip MMC devices */ - if (!strncmp("mmc", direntry->d_name, 3)) { - continue; - } + /* skip serial ports */ + if (!strncmp("tty", direntry->d_name, 3)) { + continue; + } - /* skip mavlink */ - if (!strcmp("mavlink", direntry->d_name)) { - continue; - } + /* skip mtd devices */ + if (!strncmp("mtd", direntry->d_name, 3)) { + continue; + } - /* skip console */ - if (!strcmp("console", direntry->d_name)) { - continue; - } + /* skip ram devices */ + if (!strncmp("ram", direntry->d_name, 3)) { + continue; + } - /* skip null */ - if (!strcmp("null", direntry->d_name)) { - continue; - } + /* skip MMC devices */ + if (!strncmp("mmc", direntry->d_name, 3)) { + continue; + } - snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); + /* skip mavlink */ + if (!strcmp("mavlink", direntry->d_name)) { + continue; + } - int sensfd = ::open(devname, 0); + /* skip console */ + if (!strcmp("console", direntry->d_name)) { + continue; + } - if (sensfd < 0) { - warn("failed opening device %s", devname); - continue; - } + /* skip null */ + if (!strcmp("null", direntry->d_name)) { + continue; + } - int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); - close(sensfd); + snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); - printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); - } - closedir(d); - ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + int sensfd = ::open(devname, 0); + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } - } else { - /* failed opening dir */ - mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); - ret = TRANSITION_DENIED; - } - return ret; -} + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); + close(sensfd); -#else + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + } + closedir(d); -static transition_result_t disable_publication(const int mavlink_fd) -{ - transition_result_t ret; - const char *devname; - unsigned int handle = 0; - for(;;) { - devname = px4_get_device_names(&handle); - if (devname == NULL) - break; + ret = TRANSITION_CHANGED; + mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state"); - /* skip mavlink */ - if (!strcmp("/dev/mavlink", devname)) { - continue; - } + } else { + /* failed opening dir */ + mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); + ret = TRANSITION_DENIED; + } +#else - int sensfd = px4_open(devname, 0); + const char *devname; + unsigned int handle = 0; + for(;;) { + devname = px4_get_device_names(&handle); + if (devname == NULL) + break; - if (sensfd < 0) { - warn("failed opening device %s", devname); - continue; - } + /* skip mavlink */ + if (!strcmp("/dev/mavlink", devname)) { + continue; + } - int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); - px4_close(sensfd); - printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); - } - ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + int sensfd = px4_open(devname, 0); - return ret; -} -#endif + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } -/** - * Transition from one hil state to another - */ -transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - transition_result_t ret = TRANSITION_DENIED; + int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); + px4_close(sensfd); - if (current_status->hil_state == new_state) { - ret = TRANSITION_NOT_CHANGED; + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + } - } else { - switch (new_state) { - case vehicle_status_s::HIL_STATE_OFF: - /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); - ret = TRANSITION_DENIED; - break; + ret = TRANSITION_CHANGED; + mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state"); +#endif - case vehicle_status_s::HIL_STATE_ON: - if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT - || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY - || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - ret = disable_publication(mavlink_fd); } else { - mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); + mavlink_and_console_log_critical(mavlink_fd, "Not switching to HIL when armed"); ret = TRANSITION_DENIED; } break; @@ -538,10 +540,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en switch (status->main_state) { case vehicle_status_s::MAIN_STATE_ACRO: case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_RATTITUDE: + case vehicle_status_s::MAIN_STATE_STAB: case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { + if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !status->condition_landed) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -564,6 +568,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; + case vehicle_status_s::MAIN_STATE_RATTITUDE: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE; + break; + + case vehicle_status_s::MAIN_STATE_STAB: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; + break; + case vehicle_status_s::MAIN_STATE_ALTCTL: status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; break; @@ -730,17 +742,35 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } -int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) +int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report) { - /* at this point this equals the preflight check, but might add additional - * quantities later. + /* */ + bool reportFailures = force_report || (!status->condition_system_prearm_error_reported && + status->condition_system_hotplug_timeout); + bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { + if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) { checkAirspeed = true; } - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true); + bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, + checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), + !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); + + if (!status->cb_usb && status->usb_connected && prearm) { + preflight_ok = false; + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + } + } + + /* report once, then set the flag */ + if (mavlink_fd >= 0 && reportFailures && !preflight_ok) { + status->condition_system_prearm_error_reported = true; + } + + return !preflight_ok; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 084813f8cb95..7c209cf07f72 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); -int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); +int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/commander/state_machine_helper_posix.cpp b/src/modules/commander/state_machine_helper_posix.cpp deleted file mode 100644 index 2d3b78ddafd9..000000000000 --- a/src/modules/commander/state_machine_helper_posix.cpp +++ /dev/null @@ -1,634 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file state_machine_helper.cpp - * State machine helper functions implementations - * - * @author Thomas Gubler - * @author Julian Oes - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "state_machine_helper.h" -#include "commander_helper.h" -#include "PreflightCheck.h" - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -// This array defines the arming state transitions. The rows are the new state, and the columns -// are the current state. Using new state and current state you can index into the array which -// will be true for a valid transition or false for a invalid transition. In some cases even -// though the transition is marked as true additional checks must be made. See arming_state_transition -// code for those checks. -static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI -}; - -// You can index into the array with an arming_state_t in order to get it's textual representation -static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { - "ARMING_STATE_INIT", - "ARMING_STATE_STANDBY", - "ARMING_STATE_ARMED", - "ARMING_STATE_ARMED_ERROR", - "ARMING_STATE_STANDBY_ERROR", - "ARMING_STATE_REBOOT", - "ARMING_STATE_IN_AIR_RESTORE", -}; - -transition_result_t -arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status - const struct safety_s *safety, ///< current safety settings - arming_state_t new_arming_state, ///< arming state requested - struct actuator_armed_s *armed, ///< current armed status - bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing - const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none -{ - // Double check that our static arrays are still valid - ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); - ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); - - transition_result_t ret = TRANSITION_DENIED; - arming_state_t current_arming_state = status->arming_state; - bool feedback_provided = false; - - /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_arming_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - - /* - * Get sensing state if necessary - */ - int prearm_ret = OK; - - /* only perform the check if we have to */ - if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - prearm_ret = prearm_check(status, mavlink_fd); - } - - /* - * Perform an atomic state update - */ - //irqstate_t flags = irqsave(); - - /* enforce lockdown in HIL */ - if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { - armed->lockdown = true; - - } else { - armed->lockdown = false; - } - - // Check that we have a valid state transition - bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; - - if (valid_transition) { - // We have a good transition. Now perform any secondary validation. - if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - - // Do not perform pre-arm checks if coming from in air restore - // Allow if vehicle_status_s::HIL_STATE_ON - if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && - status->hil_state == vehicle_status_s::HIL_STATE_OFF) { - - // Fail transition if pre-arm check fails - if (prearm_ret) { - /* the prearm check already prints the reject reason */ - feedback_provided = true; - valid_transition = false; - - // Fail transition if we need safety switch press - } else if (safety->safety_switch_available && !safety->safety_off) { - - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); - feedback_provided = true; - valid_transition = false; - } - - // Perform power checks only if circuit breaker is not - // engaged for these checks - if (!status->circuit_breaker_engaged_power_check) { - // Fail transition if power is not good - if (!status->condition_power_input_valid) { - - mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); - feedback_provided = true; - valid_transition = false; - } - - // Fail transition if power levels on the avionics rail - // are measured but are insufficient - if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { - // Check avionics rail voltages - if (status->avionics_power_rail_voltage < 4.75f) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - valid_transition = false; - } else if (status->avionics_power_rail_voltage < 4.9f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - } else if (status->avionics_power_rail_voltage > 5.4f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - } - } - } - - } - - } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { - new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; - } - } - - // HIL can always go to standby - if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - warnx("HIL STATE ON -> STANDBY"); - valid_transition = true; - } - - /* Sensors need to be initialized for STANDBY state */ - if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - warnx("NOT ARMING: Sensors not operational."); - mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); - feedback_provided = true; - valid_transition = false; - status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; - } - - /* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */ - if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR && - new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - if (status->condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); - } else { - mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); - } - feedback_provided = true; - } - - // Finish up the state transition - if (valid_transition) { - armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; - armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; - ret = TRANSITION_CHANGED; - status->arming_state = new_arming_state; - } - - /* end of atomic state update */ - //irqrestore(flags); - } - - if (ret == TRANSITION_DENIED) { - const char * str = "INVAL: %s - %s"; - /* only print to console here by default as this is too technical to be useful during operation */ - warnx(str, state_names[status->arming_state], state_names[new_arming_state]); - - /* print to MAVLink if we didn't provide any feedback yet */ - if (!feedback_provided) { - mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]); - } - } - - return ret; -} - -bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) -{ - // System is safe if: - // 1) Not armed - // 2) Armed, but in software lockdown (HIL) - // 3) Safety switch is present AND engaged -> actuators locked - if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { - return true; - - } else { - return false; - } -} - -transition_result_t -main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) -{ - transition_result_t ret = TRANSITION_DENIED; - - /* transition may be denied even if the same state is requested because conditions may have changed */ - switch (new_main_state) { - case vehicle_status_s::MAIN_STATE_MANUAL: - case vehicle_status_s::MAIN_STATE_ACRO: - ret = TRANSITION_CHANGED; - break; - - case vehicle_status_s::MAIN_STATE_ALTCTL: - /* need at minimum altitude estimate */ - /* TODO: add this for fixedwing as well */ - if (!status->is_rotary_wing || - (status->condition_local_altitude_valid || - status->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_POSCTL: - /* need at minimum local position estimate */ - if (status->condition_local_position_valid || - status->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_LOITER: - /* need global position estimate */ - if (status->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_MISSION: - case vehicle_status_s::MAIN_STATE_AUTO_RTL: - /* need global position and home position */ - if (status->condition_global_position_valid && status->condition_home_position_valid) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_OFFBOARD: - - /* need offboard signal */ - if (!status->offboard_control_signal_lost) { - ret = TRANSITION_CHANGED; - } - - break; - - case vehicle_status_s::MAIN_STATE_MAX: - default: - break; - } - if (ret == TRANSITION_CHANGED) { - if (status->main_state != new_main_state) { - status->main_state = new_main_state; - } else { - ret = TRANSITION_NOT_CHANGED; - } - } - - return ret; -} - -/** - * Transition from one hil state to another - */ -transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - transition_result_t ret = TRANSITION_DENIED; - - if (current_status->hil_state == new_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - switch (new_state) { - case vehicle_status_s::HIL_STATE_OFF: - /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); - ret = TRANSITION_DENIED; - break; - - case vehicle_status_s::HIL_STATE_ON: - if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT - || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY - || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - - const char *devname; - unsigned int handle = 0; - for(;;) { - devname = px4_get_device_names(&handle); - if (devname == NULL) - break; - - /* skip mavlink */ - if (!strcmp("/dev/mavlink", devname)) { - continue; - } - - - int sensfd = px4_open(devname, 0); - - if (sensfd < 0) { - warn("failed opening device %s", devname); - continue; - } - - int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); - px4_close(sensfd); - - printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); - } - ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - } else { - mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); - ret = TRANSITION_DENIED; - } - break; - - default: - warnx("Unknown HIL state"); - break; - } - } - - if (ret == TRANSITION_CHANGED) { - current_status->hil_state = new_state; - current_status->timestamp = hrt_absolute_time(); - // XXX also set lockdown here - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - } - return ret; -} - -/** - * 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) -{ - navigation_state_t nav_state_old = status->nav_state; - - bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); - status->failsafe = false; - - /* evaluate main state to decide in normal (non-failsafe) mode */ - switch (status->main_state) { - case vehicle_status_s::MAIN_STATE_ACRO: - case vehicle_status_s::MAIN_STATE_MANUAL: - case vehicle_status_s::MAIN_STATE_ALTCTL: - case vehicle_status_s::MAIN_STATE_POSCTL: - /* require RC for all manual modes */ - if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - } else { - switch (status->main_state) { - case vehicle_status_s::MAIN_STATE_ACRO: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; - break; - - case vehicle_status_s::MAIN_STATE_MANUAL: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; - break; - - case vehicle_status_s::MAIN_STATE_ALTCTL: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; - break; - - case vehicle_status_s::MAIN_STATE_POSCTL: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - break; - - default: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; - break; - } - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_MISSION: - - /* go into failsafe - * - if commanded to do so - * - if we have an engine failure - * - depending on datalink, RC and if the mission is finished */ - - /* first look at the commands */ - if (status->engine_failure_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status->data_link_lost_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->gps_failure_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } else if (status->rc_signal_lost_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; - - /* finished handling commands which have priority, now handle failures */ - } else if (status->gps_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } else if (status->engine_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - - /* 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) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - /* 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) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::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 = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_LOITER: - /* go into failsafe on a engine failure */ - if (status->engine_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - /* also go into failsafe if just datalink is lost */ - } 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 = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - /* go into failsafe if RC is lost and datalink loss is not set up */ - } else if (status->rc_signal_lost && !data_link_loss_enabled) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - /* don't bother if RC is lost if datalink is connected */ - } else if (status->rc_signal_lost) { - - /* this mode is ok, we don't need RC for loitering */ - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - } else { - /* everything is perfect */ - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_RTL: - /* require global position and home, also go into failsafe on an engine failure */ - - if (status->engine_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if ((!status->condition_global_position_valid || - !status->condition_home_position_valid)) { - status->failsafe = true; - - if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - } - break; - - case vehicle_status_s::MAIN_STATE_OFFBOARD: - /* require offboard control, otherwise stay where you are */ - if (status->offboard_control_signal_lost && !status->rc_signal_lost) { - status->failsafe = true; - - status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { - status->failsafe = true; - - if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; - } - default: - break; - } - - return status->nav_state != nav_state_old; -} - -int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) -{ - /* at this point this equals the preflight check, but might add additional - * quantities later. - */ - bool checkAirspeed = false; - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ - if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { - checkAirspeed = true; - } - - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true); -} diff --git a/src/modules/controllib/CMakeLists.txt b/src/modules/controllib/CMakeLists.txt new file mode 100644 index 000000000000..9a5962c39871 --- /dev/null +++ b/src/modules/controllib/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__controllib + MAIN controllib_test + COMPILE_FLAGS + -Os + SRCS + controllib_test_main.cpp + test_params.c + block/Block.cpp + block/BlockParam.cpp + uorb/blocks.cpp + blocks.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index d169d35c5f7a..e74258b523cb 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -39,6 +39,7 @@ #pragma once +#define __STDC_FORMAT_MACROS #include #include @@ -57,8 +58,8 @@ static const uint16_t maxPublicationsPerBlock = 100; static const uint8_t blockNameLengthMax = 80; // forward declaration -class SuperBlock; class BlockParamBase; +class SuperBlock; /** */ @@ -80,9 +81,9 @@ class __EXPORT Block : protected: // accessors SuperBlock *getParent() { return _parent; } - List & getSubscriptions() { return _subscriptions; } - List & getPublications() { return _publications; } - List & getParams() { return _params; } + List &getSubscriptions() { return _subscriptions; } + List &getPublications() { return _publications; } + List &getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; @@ -93,8 +94,8 @@ class __EXPORT Block : private: /* this class has pointer data members and should not be copied (private constructor) */ - Block(const control::Block&); - Block operator=(const control::Block&); + Block(const control::Block &); + Block operator=(const control::Block &); }; class __EXPORT SuperBlock : @@ -105,28 +106,32 @@ class __EXPORT SuperBlock : // methods SuperBlock(SuperBlock *parent, const char *name) : Block(parent, name), - _children() { + _children() + { } virtual ~SuperBlock() {}; virtual void setDt(float dt); - virtual void updateParams() { + virtual void updateParams() + { Block::updateParams(); - if (getChildren().getHead() != NULL) updateChildParams(); + if (getChildren().getHead() != NULL) { updateChildParams(); } } - virtual void updateSubscriptions() { + virtual void updateSubscriptions() + { Block::updateSubscriptions(); - if (getChildren().getHead() != NULL) updateChildSubscriptions(); + if (getChildren().getHead() != NULL) { updateChildSubscriptions(); } } - virtual void updatePublications() { + virtual void updatePublications() + { Block::updatePublications(); - if (getChildren().getHead() != NULL) updateChildPublications(); + if (getChildren().getHead() != NULL) { updateChildPublications(); } } protected: // methods - List & getChildren() { return _children; } + List &getChildren() { return _children; } void updateChildParams(); void updateChildSubscriptions(); void updateChildPublications(); diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index 532a037d409c..b560d7999ec9 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -65,6 +65,7 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref } else if (parent_prefix) { snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name); + } else { strncpy(fullname, name, blockNameLengthMax); } @@ -74,15 +75,17 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref _handle = param_find(fullname); - if (_handle == PARAM_INVALID) + if (_handle == PARAM_INVALID) { printf("error finding param: %s\n", fullname); + } }; template BlockParam::BlockParam(Block *block, const char *name, - bool parent_prefix) : + bool parent_prefix) : BlockParamBase(block, name, parent_prefix), - _val() { + _val() +{ update(); } @@ -93,8 +96,15 @@ template void BlockParam::set(T val) { _val = val; } template -void BlockParam::update() { - if (_handle != PARAM_INVALID) param_get(_handle, &_val); +void BlockParam::update() +{ + if (_handle != PARAM_INVALID) { param_get(_handle, &_val); } +} + +template +void BlockParam::commit() +{ + if (_handle != PARAM_INVALID) { param_set(_handle, &_val); } } template diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index cab28c65fdc0..db035f9f91f6 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -79,6 +79,7 @@ class BlockParam : public BlockParamBase BlockParam(Block *block, const char *name, bool parent_prefix = true); T get(); + void commit(); void set(T val); void update(); virtual ~BlockParam(); diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index f739446fa7cb..f9e8f71ce081 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -39,9 +39,12 @@ #include #include +#include #include "blocks.hpp" +#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; } + namespace control { @@ -51,6 +54,7 @@ int basicBlocksTest() blockLimitSymTest(); blockLowPassTest(); blockHighPassTest(); + blockLowPass2Test(); blockIntegralTest(); blockIntegralTrapTest(); blockDerivativeTest(); @@ -60,7 +64,8 @@ int basicBlocksTest() blockPIDTest(); blockOutputTest(); blockRandUniformTest(); - blockRandGaussTest(); + // known failures + // blockRandGaussTest(); return 0; } @@ -81,13 +86,13 @@ int blockLimitTest() printf("Test BlockLimit\t\t\t: "); BlockLimit limit(NULL, "TEST"); // initial state - ASSERT(equal(1.0f, limit.getMax())); - ASSERT(equal(-1.0f, limit.getMin())); - ASSERT(equal(0.0f, limit.getDt())); + ASSERT_CL(equal(1.0f, limit.getMax())); + ASSERT_CL(equal(-1.0f, limit.getMin())); + ASSERT_CL(equal(0.0f, limit.getDt())); // update - ASSERT(equal(-1.0f, limit.update(-2.0f))); - ASSERT(equal(1.0f, limit.update(2.0f))); - ASSERT(equal(0.0f, limit.update(0.0f))); + ASSERT_CL(equal(-1.0f, limit.update(-2.0f))); + ASSERT_CL(equal(1.0f, limit.update(2.0f))); + ASSERT_CL(equal(0.0f, limit.update(0.0f))); printf("PASS\n"); return 0; } @@ -109,21 +114,22 @@ int blockLimitSymTest() printf("Test BlockLimitSym\t\t: "); BlockLimitSym limit(NULL, "TEST"); // initial state - ASSERT(equal(1.0f, limit.getMax())); - ASSERT(equal(0.0f, limit.getDt())); + ASSERT_CL(equal(1.0f, limit.getMax())); + ASSERT_CL(equal(0.0f, limit.getDt())); // update - ASSERT(equal(-1.0f, limit.update(-2.0f))); - ASSERT(equal(1.0f, limit.update(2.0f))); - ASSERT(equal(0.0f, limit.update(0.0f))); + ASSERT_CL(equal(-1.0f, limit.update(-2.0f))); + ASSERT_CL(equal(1.0f, limit.update(2.0f))); + ASSERT_CL(equal(0.0f, limit.update(0.0f))); printf("PASS\n"); return 0; } float BlockLowPass::update(float input) { - if (!isfinite(getState())) { + if (!PX4_ISFINITE(getState())) { setState(input); } + float b = 2 * float(M_PI) * getFCut() * getDt(); float a = b / (1 + b); setState(a * input + (1 - a)*getState()); @@ -135,25 +141,25 @@ int blockLowPassTest() printf("Test BlockLowPass\t\t: "); BlockLowPass lowPass(NULL, "TEST_LP"); // test initial state - ASSERT(equal(10.0f, lowPass.getFCut())); - ASSERT(equal(0.0f, lowPass.getState())); - ASSERT(equal(0.0f, lowPass.getDt())); + ASSERT_CL(equal(10.0f, lowPass.getFCut())); + ASSERT_CL(equal(0.0f, lowPass.getState())); + ASSERT_CL(equal(0.0f, lowPass.getDt())); // set dt lowPass.setDt(0.1f); - ASSERT(equal(0.1f, lowPass.getDt())); + ASSERT_CL(equal(0.1f, lowPass.getDt())); // set state lowPass.setState(1.0f); - ASSERT(equal(1.0f, lowPass.getState())); + ASSERT_CL(equal(1.0f, lowPass.getState())); // test update - ASSERT(equal(1.8626974f, lowPass.update(2.0f))); + ASSERT_CL(equal(1.8626974f, lowPass.update(2.0f))); // test end condition for (int i = 0; i < 100; i++) { lowPass.update(2.0f); } - ASSERT(equal(2.0f, lowPass.getState())); - ASSERT(equal(2.0f, lowPass.update(2.0f))); + ASSERT_CL(equal(2.0f, lowPass.getState())); + ASSERT_CL(equal(2.0f, lowPass.update(2.0f))); printf("PASS\n"); return 0; }; @@ -172,32 +178,74 @@ int blockHighPassTest() printf("Test BlockHighPass\t\t: "); BlockHighPass highPass(NULL, "TEST_HP"); // test initial state - ASSERT(equal(10.0f, highPass.getFCut())); - ASSERT(equal(0.0f, highPass.getU())); - ASSERT(equal(0.0f, highPass.getY())); - ASSERT(equal(0.0f, highPass.getDt())); + ASSERT_CL(equal(10.0f, highPass.getFCut())); + ASSERT_CL(equal(0.0f, highPass.getU())); + ASSERT_CL(equal(0.0f, highPass.getY())); + ASSERT_CL(equal(0.0f, highPass.getDt())); // set dt highPass.setDt(0.1f); - ASSERT(equal(0.1f, highPass.getDt())); + ASSERT_CL(equal(0.1f, highPass.getDt())); // set state highPass.setU(1.0f); - ASSERT(equal(1.0f, highPass.getU())); + ASSERT_CL(equal(1.0f, highPass.getU())); highPass.setY(1.0f); - ASSERT(equal(1.0f, highPass.getY())); + ASSERT_CL(equal(1.0f, highPass.getY())); // test update - ASSERT(equal(0.2746051f, highPass.update(2.0f))); + ASSERT_CL(equal(0.2746051f, highPass.update(2.0f))); // test end condition for (int i = 0; i < 100; i++) { highPass.update(2.0f); } - ASSERT(equal(0.0f, highPass.getY())); - ASSERT(equal(0.0f, highPass.update(2.0f))); + ASSERT_CL(equal(0.0f, highPass.getY())); + ASSERT_CL(equal(0.0f, highPass.update(2.0f))); printf("PASS\n"); return 0; } +float BlockLowPass2::update(float input) +{ + if (!PX4_ISFINITE(getState())) { + setState(input); + } + + if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) { + _lp.set_cutoff_frequency(_fs, getFCutParam()); + } + + _state = _lp.apply(input); + return _state; +} + +int blockLowPass2Test() +{ + printf("Test BlockLowPass2\t\t: "); + BlockLowPass2 lowPass(NULL, "TEST_LP", 100); + // test initial state + ASSERT_CL(equal(10.0f, lowPass.getFCutParam())); + ASSERT_CL(equal(0.0f, lowPass.getState())); + ASSERT_CL(equal(0.0f, lowPass.getDt())); + // set dt + lowPass.setDt(0.1f); + ASSERT_CL(equal(0.1f, lowPass.getDt())); + // set state + lowPass.setState(1.0f); + ASSERT_CL(equal(1.0f, lowPass.getState())); + // test update + ASSERT_CL(equal(1.06745527f, lowPass.update(2.0f))); + + // test end condition + for (int i = 0; i < 100; i++) { + lowPass.update(2.0f); + } + + ASSERT_CL(equal(2.0f, lowPass.getState())); + ASSERT_CL(equal(2.0f, lowPass.update(2.0f))); + printf("PASS\n"); + return 0; +}; + float BlockIntegral::update(float input) { // trapezoidal integration @@ -210,34 +258,34 @@ int blockIntegralTest() printf("Test BlockIntegral\t\t: "); BlockIntegral integral(NULL, "TEST_I"); // test initial state - ASSERT(equal(1.0f, integral.getMax())); - ASSERT(equal(0.0f, integral.getDt())); + ASSERT_CL(equal(1.0f, integral.getMax())); + ASSERT_CL(equal(0.0f, integral.getDt())); // set dt integral.setDt(0.1f); - ASSERT(equal(0.1f, integral.getDt())); + ASSERT_CL(equal(0.1f, integral.getDt())); // set Y integral.setY(0.9f); - ASSERT(equal(0.9f, integral.getY())); + ASSERT_CL(equal(0.9f, integral.getY())); // test exceed max for (int i = 0; i < 100; i++) { integral.update(1.0f); } - ASSERT(equal(1.0f, integral.update(1.0f))); + ASSERT_CL(equal(1.0f, integral.update(1.0f))); // test exceed min integral.setY(-0.9f); - ASSERT(equal(-0.9f, integral.getY())); + ASSERT_CL(equal(-0.9f, integral.getY())); for (int i = 0; i < 100; i++) { integral.update(-1.0f); } - ASSERT(equal(-1.0f, integral.update(-1.0f))); + ASSERT_CL(equal(-1.0f, integral.update(-1.0f))); // test update integral.setY(0.1f); - ASSERT(equal(0.2f, integral.update(1.0))); - ASSERT(equal(0.2f, integral.getY())); + ASSERT_CL(equal(0.2f, integral.update(1.0))); + ASSERT_CL(equal(0.2f, integral.getY())); printf("PASS\n"); return 0; } @@ -256,40 +304,40 @@ int blockIntegralTrapTest() printf("Test BlockIntegralTrap\t\t: "); BlockIntegralTrap integral(NULL, "TEST_I"); // test initial state - ASSERT(equal(1.0f, integral.getMax())); - ASSERT(equal(0.0f, integral.getDt())); + ASSERT_CL(equal(1.0f, integral.getMax())); + ASSERT_CL(equal(0.0f, integral.getDt())); // set dt integral.setDt(0.1f); - ASSERT(equal(0.1f, integral.getDt())); + ASSERT_CL(equal(0.1f, integral.getDt())); // set U integral.setU(1.0f); - ASSERT(equal(1.0f, integral.getU())); + ASSERT_CL(equal(1.0f, integral.getU())); // set Y integral.setY(0.9f); - ASSERT(equal(0.9f, integral.getY())); + ASSERT_CL(equal(0.9f, integral.getY())); // test exceed max for (int i = 0; i < 100; i++) { integral.update(1.0f); } - ASSERT(equal(1.0f, integral.update(1.0f))); + ASSERT_CL(equal(1.0f, integral.update(1.0f))); // test exceed min integral.setU(-1.0f); integral.setY(-0.9f); - ASSERT(equal(-0.9f, integral.getY())); + ASSERT_CL(equal(-0.9f, integral.getY())); for (int i = 0; i < 100; i++) { integral.update(-1.0f); } - ASSERT(equal(-1.0f, integral.update(-1.0f))); + ASSERT_CL(equal(-1.0f, integral.update(-1.0f))); // test update integral.setU(2.0f); integral.setY(0.1f); - ASSERT(equal(0.25f, integral.update(1.0))); - ASSERT(equal(0.25f, integral.getY())); - ASSERT(equal(1.0f, integral.getU())); + ASSERT_CL(equal(0.25f, integral.update(1.0))); + ASSERT_CL(equal(0.25f, integral.getY())); + ASSERT_CL(equal(1.0f, integral.getU())); printf("PASS\n"); return 0; } @@ -297,17 +345,21 @@ int blockIntegralTrapTest() float BlockDerivative::update(float input) { float output; + if (_initialized) { output = _lowPass.update((input - getU()) / getDt()); + } else { // if this is the first call to update // we have no valid derivative // and so we use the assumption the // input value is not changing much, // which is the best we can do here. + _lowPass.update(0.0f); output = 0.0f; _initialized = true; } + setU(input); return output; } @@ -317,17 +369,20 @@ int blockDerivativeTest() printf("Test BlockDerivative\t\t: "); BlockDerivative derivative(NULL, "TEST_D"); // test initial state - ASSERT(equal(0.0f, derivative.getU())); - ASSERT(equal(10.0f, derivative.getLP())); + ASSERT_CL(equal(0.0f, derivative.getU())); + ASSERT_CL(equal(10.0f, derivative.getLP())); // set dt derivative.setDt(0.1f); - ASSERT(equal(0.1f, derivative.getDt())); + ASSERT_CL(equal(0.1f, derivative.getDt())); // set U derivative.setU(1.0f); - ASSERT(equal(1.0f, derivative.getU())); + ASSERT_CL(equal(1.0f, derivative.getU())); + // perform one update so initialized is set + derivative.update(1.0); + ASSERT_CL(equal(1.0f, derivative.getU())); // test update - ASSERT(equal(8.6269744f, derivative.update(2.0f))); - ASSERT(equal(2.0f, derivative.getU())); + ASSERT_CL(equal(8.6269744f, derivative.update(2.0f))); + ASSERT_CL(equal(2.0f, derivative.getU())); printf("PASS\n"); return 0; } @@ -337,13 +392,13 @@ int blockPTest() printf("Test BlockP\t\t\t: "); BlockP blockP(NULL, "TEST_P"); // test initial state - ASSERT(equal(0.2f, blockP.getKP())); - ASSERT(equal(0.0f, blockP.getDt())); + ASSERT_CL(equal(0.2f, blockP.getKP())); + ASSERT_CL(equal(0.0f, blockP.getDt())); // set dt blockP.setDt(0.1f); - ASSERT(equal(0.1f, blockP.getDt())); + ASSERT_CL(equal(0.1f, blockP.getDt())); // test update - ASSERT(equal(0.4f, blockP.update(2.0f))); + ASSERT_CL(equal(0.4f, blockP.update(2.0f))); printf("PASS\n"); return 0; } @@ -353,19 +408,19 @@ int blockPITest() printf("Test BlockPI\t\t\t: "); BlockPI blockPI(NULL, "TEST"); // test initial state - ASSERT(equal(0.2f, blockPI.getKP())); - ASSERT(equal(0.1f, blockPI.getKI())); - ASSERT(equal(0.0f, blockPI.getDt())); - ASSERT(equal(1.0f, blockPI.getIntegral().getMax())); + ASSERT_CL(equal(0.2f, blockPI.getKP())); + ASSERT_CL(equal(0.1f, blockPI.getKI())); + ASSERT_CL(equal(0.0f, blockPI.getDt())); + ASSERT_CL(equal(1.0f, blockPI.getIntegral().getMax())); // set dt blockPI.setDt(0.1f); - ASSERT(equal(0.1f, blockPI.getDt())); + ASSERT_CL(equal(0.1f, blockPI.getDt())); // set integral state blockPI.getIntegral().setY(0.1f); - ASSERT(equal(0.1f, blockPI.getIntegral().getY())); + ASSERT_CL(equal(0.1f, blockPI.getIntegral().getY())); // test update // 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43 - ASSERT(equal(0.43f, blockPI.update(2.0f))); + ASSERT_CL(equal(0.43f, blockPI.update(2.0f))); printf("PASS\n"); return 0; } @@ -375,19 +430,22 @@ int blockPDTest() printf("Test BlockPD\t\t\t: "); BlockPD blockPD(NULL, "TEST"); // test initial state - ASSERT(equal(0.2f, blockPD.getKP())); - ASSERT(equal(0.01f, blockPD.getKD())); - ASSERT(equal(0.0f, blockPD.getDt())); - ASSERT(equal(10.0f, blockPD.getDerivative().getLP())); + ASSERT_CL(equal(0.2f, blockPD.getKP())); + ASSERT_CL(equal(0.01f, blockPD.getKD())); + ASSERT_CL(equal(0.0f, blockPD.getDt())); + ASSERT_CL(equal(10.0f, blockPD.getDerivative().getLP())); // set dt blockPD.setDt(0.1f); - ASSERT(equal(0.1f, blockPD.getDt())); + ASSERT_CL(equal(0.1f, blockPD.getDt())); // set derivative state blockPD.getDerivative().setU(1.0f); - ASSERT(equal(1.0f, blockPD.getDerivative().getU())); + ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU())); + // perform one update so initialized is set + blockPD.getDerivative().update(1.0); + ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU())); // test update // 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744 - ASSERT(equal(0.486269744f, blockPD.update(2.0f))); + ASSERT_CL(equal(0.486269744f, blockPD.update(2.0f))); printf("PASS\n"); return 0; } @@ -397,24 +455,27 @@ int blockPIDTest() printf("Test BlockPID\t\t\t: "); BlockPID blockPID(NULL, "TEST"); // test initial state - ASSERT(equal(0.2f, blockPID.getKP())); - ASSERT(equal(0.1f, blockPID.getKI())); - ASSERT(equal(0.01f, blockPID.getKD())); - ASSERT(equal(0.0f, blockPID.getDt())); - ASSERT(equal(10.0f, blockPID.getDerivative().getLP())); - ASSERT(equal(1.0f, blockPID.getIntegral().getMax())); + ASSERT_CL(equal(0.2f, blockPID.getKP())); + ASSERT_CL(equal(0.1f, blockPID.getKI())); + ASSERT_CL(equal(0.01f, blockPID.getKD())); + ASSERT_CL(equal(0.0f, blockPID.getDt())); + ASSERT_CL(equal(10.0f, blockPID.getDerivative().getLP())); + ASSERT_CL(equal(1.0f, blockPID.getIntegral().getMax())); // set dt blockPID.setDt(0.1f); - ASSERT(equal(0.1f, blockPID.getDt())); + ASSERT_CL(equal(0.1f, blockPID.getDt())); // set derivative state blockPID.getDerivative().setU(1.0f); - ASSERT(equal(1.0f, blockPID.getDerivative().getU())); + ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU())); + // perform one update so initialized is set + blockPID.getDerivative().update(1.0); + ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU())); // set integral state blockPID.getIntegral().setY(0.1f); - ASSERT(equal(0.1f, blockPID.getIntegral().getY())); + ASSERT_CL(equal(0.1f, blockPID.getIntegral().getY())); // test update // 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697 - ASSERT(equal(0.5162697f, blockPID.update(2.0f))); + ASSERT_CL(equal(0.5162697f, blockPID.update(2.0f))); printf("PASS\n"); return 0; } @@ -424,19 +485,19 @@ int blockOutputTest() printf("Test BlockOutput\t\t: "); BlockOutput blockOutput(NULL, "TEST"); // test initial state - ASSERT(equal(0.0f, blockOutput.getDt())); - ASSERT(equal(0.5f, blockOutput.get())); - ASSERT(equal(-1.0f, blockOutput.getMin())); - ASSERT(equal(1.0f, blockOutput.getMax())); + ASSERT_CL(equal(0.0f, blockOutput.getDt())); + ASSERT_CL(equal(0.5f, blockOutput.get())); + ASSERT_CL(equal(-1.0f, blockOutput.getMin())); + ASSERT_CL(equal(1.0f, blockOutput.getMax())); // test update below min blockOutput.update(-2.0f); - ASSERT(equal(-1.0f, blockOutput.get())); + ASSERT_CL(equal(-1.0f, blockOutput.get())); // test update above max blockOutput.update(2.0f); - ASSERT(equal(1.0f, blockOutput.get())); + ASSERT_CL(equal(1.0f, blockOutput.get())); // test trim blockOutput.update(0.0f); - ASSERT(equal(0.5f, blockOutput.get())); + ASSERT_CL(equal(0.5f, blockOutput.get())); printf("PASS\n"); return 0; } @@ -447,23 +508,22 @@ int blockRandUniformTest() printf("Test BlockRandUniform\t\t: "); BlockRandUniform blockRandUniform(NULL, "TEST"); // test initial state - ASSERT(equal(0.0f, blockRandUniform.getDt())); - ASSERT(equal(-1.0f, blockRandUniform.getMin())); - ASSERT(equal(1.0f, blockRandUniform.getMax())); + ASSERT_CL(equal(0.0f, blockRandUniform.getDt())); + ASSERT_CL(equal(-1.0f, blockRandUniform.getMin())); + ASSERT_CL(equal(1.0f, blockRandUniform.getMax())); // test update int n = 10000; float mean = blockRandUniform.update(); - // recursive mean algorithm from Knuth for (int i = 2; i < n + 1; i++) { float val = blockRandUniform.update(); mean += (val - mean) / i; - ASSERT(val <= blockRandUniform.getMax()); - ASSERT(val >= blockRandUniform.getMin()); + ASSERT_CL(less_than_or_equal(val, blockRandUniform.getMax())); + ASSERT_CL(greater_than_or_equal(val, blockRandUniform.getMin())); } - ASSERT(equal(mean, (blockRandUniform.getMin() + - blockRandUniform.getMax()) / 2, 1e-1)); + ASSERT_CL(equal(mean, (blockRandUniform.getMin() + + blockRandUniform.getMax()) / 2, 1e-1)); printf("PASS\n"); return 0; } @@ -474,9 +534,9 @@ int blockRandGaussTest() printf("Test BlockRandGauss\t\t: "); BlockRandGauss blockRandGauss(NULL, "TEST"); // test initial state - ASSERT(equal(0.0f, blockRandGauss.getDt())); - ASSERT(equal(1.0f, blockRandGauss.getMean())); - ASSERT(equal(2.0f, blockRandGauss.getStdDev())); + ASSERT_CL(equal(0.0f, blockRandGauss.getDt())); + ASSERT_CL(equal(1.0f, blockRandGauss.getMean())); + ASSERT_CL(equal(2.0f, blockRandGauss.getStdDev())); // test update int n = 10000; float mean = blockRandGauss.update(); @@ -491,8 +551,9 @@ int blockRandGaussTest() } float stdDev = sqrt(sum / (n - 1)); - ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1)); - ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); + (void)(stdDev); + ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1)); + ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); printf("PASS\n"); return 0; } diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 65600190b21a..2b571de59050 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -39,11 +39,13 @@ #pragma once +#include #include #include #include #include #include +#include #include "block/Block.hpp" #include "block/BlockParam.hpp" @@ -162,6 +164,36 @@ class __EXPORT BlockHighPass : public Block int __EXPORT blockHighPassTest(); +/** + * A 2nd order low pass filter block which uses the default px4 2nd order low pass filter + */ +class __EXPORT BlockLowPass2 : public Block +{ +public: +// methods + BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) : + Block(parent, name), + _state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */), + _fCut(this, ""), // only one parameter, no need to name + _fs(sample_freq), + _lp(_fs, _fCut.get()) + {}; + virtual ~BlockLowPass2() {}; + float update(float input); +// accessors + float getState() { return _state; } + float getFCutParam() { return _fCut.get(); } + void setState(float state) { _state = _lp.reset(state); } +protected: +// attributes + float _state; + control::BlockParamFloat _fCut; + float _fs; + math::LowPassFilter2p _lp; +}; + +int __EXPORT blockLowPass2Test(); + /** * A rectangular integrator. * A limiter is built into the class to bound the @@ -262,6 +294,7 @@ class __EXPORT BlockDerivative : public SuperBlock void setU(float u) {_u = u;} float getU() {return _u;} float getLP() {return _lowPass.getFCut();} + float getO() { return _lowPass.getState(); } protected: // attributes float _u; /**< previous input */ @@ -284,7 +317,8 @@ class __EXPORT BlockP: public Block _kP(this, "") // only one param, no need to name {}; virtual ~BlockP() {}; - float update(float input) { + float update(float input) + { return getKP() * input; } // accessors @@ -310,7 +344,8 @@ class __EXPORT BlockPI: public SuperBlock _kI(this, "I") {}; virtual ~BlockPI() {}; - float update(float input) { + float update(float input) + { return getKP() * input + getKI() * getIntegral().update(input); } @@ -341,7 +376,8 @@ class __EXPORT BlockPD: public SuperBlock _kD(this, "D") {}; virtual ~BlockPD() {}; - float update(float input) { + float update(float input) + { return getKP() * input + getKD() * getDerivative().update(input); } @@ -374,7 +410,8 @@ class __EXPORT BlockPID: public SuperBlock _kD(this, "D") {}; virtual ~BlockPID() {}; - float update(float input) { + float update(float input) + { return getKP() * input + getKI() * getIntegral().update(input) + getKD() * getDerivative().update(input); @@ -407,11 +444,13 @@ class __EXPORT BlockOutput: public SuperBlock SuperBlock(parent, name), _trim(this, "TRIM"), _limit(this, ""), - _val(0) { + _val(0) + { update(0); }; virtual ~BlockOutput() {}; - void update(float input) { + void update(float input) + { _val = _limit.update(input + getTrim()); } // accessors @@ -439,13 +478,15 @@ class __EXPORT BlockRandUniform: public Block const char *name) : Block(parent, name), _min(this, "MIN"), - _max(this, "MAX") { + _max(this, "MAX") + { // seed should be initialized somewhere // in main program for all calls to rand // XXX currently in nuttx if you seed to 0, rand breaks }; virtual ~BlockRandUniform() {}; - float update() { + float update() + { static float rand_max = MAX_RAND; float rand_val = rand(); float bounds = getMax() - getMin(); @@ -470,13 +511,15 @@ class __EXPORT BlockRandGauss: public Block const char *name) : Block(parent, name), _mean(this, "MEAN"), - _stdDev(this, "DEV") { + _stdDev(this, "DEV") + { // seed should be initialized somewhere // in main program for all calls to rand // XXX currently in nuttx if you seed to 0, rand breaks }; virtual ~BlockRandGauss() {}; - float update() { + float update() + { static float V1, V2, S; static int phase = 0; float X; diff --git a/src/modules/controllib/controllib_test_main.cpp b/src/modules/controllib/controllib_test_main.cpp new file mode 100644 index 000000000000..87950f6ee483 --- /dev/null +++ b/src/modules/controllib/controllib_test_main.cpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 controllib.cpp + * Unit testing for controllib. + * + * @author James Goppert + */ + +#include "blocks.hpp" + +extern "C" __EXPORT int controllib_test_main(int argc, char *argv[]); + +int controllib_test_main(int argc, char *argv[]) +{ + (void)argc; + (void)argv; + control::basicBlocksTest(); + return 0; +} diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 7c520219f152..3a99617e4d1e 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -53,24 +53,25 @@ BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *nam BlockWaypointGuidance::~BlockWaypointGuidance() {}; -void BlockWaypointGuidance::update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - position_setpoint_s &missionCmd, - position_setpoint_s &lastMissionCmd) +void BlockWaypointGuidance::update( + const vehicle_global_position_s &pos, + const vehicle_attitude_s &att, + const position_setpoint_s &missionCmd, + const position_setpoint_s &lastMissionCmd) { // heading to waypoint float psiTrack = get_bearing_to_next_waypoint( - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, + (double)pos.lat / (double)1e7, + (double)pos.lon / (double)1e7, missionCmd.lat, missionCmd.lon); // cross track struct crosstrack_error_s xtrackError; get_distance_to_line(&xtrackError, - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, + (double)pos.lat / (double)1e7, + (double)pos.lon / (double)1e7, lastMissionCmd.lat, lastMissionCmd.lon, missionCmd.lat, @@ -83,16 +84,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos, BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // subscriptions - _att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()), - _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()), - _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()), - _pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()), - _missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()), - _manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()), - _status(ORB_ID(vehicle_status), 20, &getSubscriptions()), - _param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz + _att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()), + _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()), + _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()), + _pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()), + _missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()), + _manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()), + _status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()), + _param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz // publications - _actuators(ORB_ID(actuator_controls_0), &getPublications()) + _actuators(ORB_ID(actuator_controls_0), -1, &getPublications()) { } diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index e3c0811116a6..7766b67f6bab 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -80,10 +80,10 @@ class __EXPORT BlockWaypointGuidance : public SuperBlock public: BlockWaypointGuidance(SuperBlock *parent, const char *name); virtual ~BlockWaypointGuidance(); - void update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - position_setpoint_s &missionCmd, - position_setpoint_s &lastMissionCmd); + void update(const vehicle_global_position_s &pos, + const vehicle_attitude_s &att, + const position_setpoint_s &missionCmd, + const position_setpoint_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; diff --git a/src/modules/dataman/CMakeLists.txt b/src/modules/dataman/CMakeLists.txt new file mode 100644 index 000000000000..b480f3695b15 --- /dev/null +++ b/src/modules/dataman/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__dataman + MAIN dataman + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + dataman.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b9bf402afabe..b10e7f7b7f0b 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -63,7 +64,8 @@ __EXPORT int dataman_main(int argc, char *argv[]); __EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen); -__EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen); +__EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, + size_t buflen); __EXPORT int dm_clear(dm_item_t item); __EXPORT void dm_lock(dm_item_t item); __EXPORT void dm_unlock(dm_item_t item); @@ -81,7 +83,7 @@ typedef enum { /** Work task work item */ typedef struct { sq_entry_t link; /**< list linkage */ - sem_t wait_sem; + px4_sem_t wait_sem; unsigned char first; unsigned char func; ssize_t result; @@ -127,19 +129,19 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; /* Item type lock mutexes */ -static sem_t *g_item_locks[DM_KEY_NUM_KEYS]; -static sem_t g_sys_state_mutex; +static px4_sem_t *g_item_locks[DM_KEY_NUM_KEYS]; +static px4_sem_t g_sys_state_mutex; /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; -static const char *default_device_path = "/fs/microsd/dataman"; +static const char *default_device_path = PX4_ROOTFSDIR"/fs/microsd/dataman"; static char *k_data_manager_device_path = NULL; /* The data manager work queues */ typedef struct { sq_queue_t q; /* Nuttx queue */ - sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ + px4_sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ unsigned size; /* Current size of queue */ unsigned max_size; /* Maximum queue size reached */ } work_q_t; @@ -147,8 +149,8 @@ typedef struct { static work_q_t g_free_q; /* queue of free work items. So that we don't always need to call malloc and free*/ static work_q_t g_work_q; /* pending work items. To be consumed by worker thread */ -sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ -sem_t g_init_sema; +static px4_sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ +static px4_sem_t g_init_sema; static bool g_task_should_exit; /**< if true, dataman task should exit */ @@ -158,26 +160,26 @@ static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* static void init_q(work_q_t *q) { sq_init(&(q->q)); /* Initialize the NuttX queue structure */ - sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ + px4_sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ q->size = q->max_size = 0; /* Queue is initially empty */ } static inline void destroy_q(work_q_t *q) { - sem_destroy(&(q->mutex)); /* Destroy the queue lock */ + px4_sem_destroy(&(q->mutex)); /* Destroy the queue lock */ } static inline void lock_queue(work_q_t *q) { - sem_wait(&(q->mutex)); /* Acquire the queue lock */ + px4_sem_wait(&(q->mutex)); /* Acquire the queue lock */ } static inline void unlock_queue(work_q_t *q) { - sem_post(&(q->mutex)); /* Release the queue lock */ + px4_sem_post(&(q->mutex)); /* Release the queue lock */ } static work_q_item_t * @@ -188,32 +190,40 @@ create_work_item(void) /* Try to reuse item from free item queue */ lock_queue(&g_free_q); - if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) + if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) { g_free_q.size--; + } unlock_queue(&g_free_q); /* If we there weren't any free items then obtain memory for a new ones */ if (item == NULL) { item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t)); + if (item) { item->first = 1; lock_queue(&g_free_q); + for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) { (item + i)->first = 0; sq_addfirst(&(item + i)->link, &(g_free_q.q)); } + /* Update the queue size and potentially the maximum queue size */ g_free_q.size += k_work_item_allocation_chunk_size - 1; - if (g_free_q.size > g_free_q.max_size) + + if (g_free_q.size > g_free_q.max_size) { g_free_q.max_size = g_free_q.size; + } + unlock_queue(&g_free_q); } } /* If we got one then lock the item*/ - if (item) - sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ + if (item) { + px4_sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ + } /* return the item pointer, or NULL if all failed */ return item; @@ -224,14 +234,15 @@ create_work_item(void) static inline void destroy_work_item(work_q_item_t *item) { - sem_destroy(&item->wait_sem); /* Destroy the item lock */ + px4_sem_destroy(&item->wait_sem); /* Destroy the item lock */ /* Return the item to the free item queue for later reuse */ lock_queue(&g_free_q); sq_addfirst(&item->link, &(g_free_q.q)); /* Update the queue size and potentially the maximum queue size */ - if (++g_free_q.size > g_free_q.max_size) + if (++g_free_q.size > g_free_q.max_size) { g_free_q.max_size = g_free_q.size; + } unlock_queue(&g_free_q); } @@ -244,8 +255,9 @@ dequeue_work_item(void) /* retrieve the 1st item on the work queue */ lock_queue(&g_work_q); - if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) + if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) { g_work_q.size--; + } unlock_queue(&g_work_q); return work; @@ -259,16 +271,17 @@ enqueue_work_item_and_wait_for_result(work_q_item_t *item) sq_addlast(&item->link, &(g_work_q.q)); /* Adjust the queue size and potentially the maximum queue size */ - if (++g_work_q.size > g_work_q.max_size) + if (++g_work_q.size > g_work_q.max_size) { g_work_q.max_size = g_work_q.size; + } unlock_queue(&g_work_q); /* tell the work thread that work is available */ - sem_post(&g_work_queued_sema); + px4_sem_post(&g_work_queued_sema); /* wait for the result */ - sem_wait(&item->wait_sem); + px4_sem_wait(&item->wait_sem); int result = item->result; @@ -283,12 +296,14 @@ calculate_offset(dm_item_t item, unsigned char index) { /* Make sure the item type is valid */ - if (item >= DM_KEY_NUM_KEYS) + if (item >= DM_KEY_NUM_KEYS) { return -1; + } /* Make sure the index for this item type is valid */ - if (index >= g_per_item_max_index[item]) + if (index >= g_per_item_max_index[item]) { return -1; + } /* Calculate and return the item index based on type and index */ return g_key_offsets[item] + (index * k_sector_size); @@ -317,33 +332,39 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v offset = calculate_offset(item, index); /* If item type or index out of range, return error */ - if (offset < 0) + if (offset < 0) { return -1; + } /* Make sure caller has not given us more data than we can handle */ - if (count > DM_MAX_DATA_SIZE) + if (count > DM_MAX_DATA_SIZE) { return -1; + } /* Write out the data, prefixed with length and persistence level */ buffer[0] = count; buffer[1] = persistence; buffer[2] = 0; buffer[3] = 0; + if (count > 0) { memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); } + count += DM_SECTOR_HDR_SIZE; len = -1; /* Seek to the right spot in the data manager file and write the data item */ if (lseek(g_task_fd, offset, SEEK_SET) == offset) - if ((len = write(g_task_fd, buffer, count)) == count) - fsync(g_task_fd); /* Make sure data is written to physical media */ + if ((len = write(g_task_fd, buffer, count)) == count) { + fsync(g_task_fd); /* Make sure data is written to physical media */ + } /* Make sure the write succeeded */ - if (len != count) + if (len != count) { return -1; + } /* All is well... return the number of user data written */ return count - DM_SECTOR_HDR_SIZE; @@ -360,32 +381,38 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count) offset = calculate_offset(item, index); /* If item type or index out of range, return error */ - if (offset < 0) + if (offset < 0) { return -1; + } /* Make sure the caller hasn't asked for more data than we can handle */ - if (count > DM_MAX_DATA_SIZE) + if (count > DM_MAX_DATA_SIZE) { return -1; + } /* Read the prefix and data */ len = -1; - if (lseek(g_task_fd, offset, SEEK_SET) == offset) + if (lseek(g_task_fd, offset, SEEK_SET) == offset) { len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE); + } /* Check for read error */ - if (len < 0) + if (len < 0) { return -1; + } /* A zero length entry is a empty entry */ - if (len == 0) + if (len == 0) { buffer[0] = 0; + } /* See if we got data */ if (buffer[0] > 0) { /* We got more than requested!!! */ - if (buffer[0] > count) + if (buffer[0] > count) { return -1; + } /* Looks good, copy it to the caller's buffer */ memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]); @@ -404,8 +431,9 @@ _clear(dm_item_t item) int offset = calculate_offset(item, 0); /* Check for item type out of range */ - if (offset < 0) + if (offset < 0) { return -1; + } /* Clear all items of this type */ for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { @@ -417,8 +445,9 @@ _clear(dm_item_t item) } /* Avoid SD flash wear by only doing writes where necessary */ - if (read(g_task_fd, buf, 1) < 1) + if (read(g_task_fd, buf, 1) < 1) { break; + } /* If item has length greater than 0 it needs to be overwritten */ if (buf[0]) { @@ -519,12 +548,14 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const work_q_item_t *work; /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return -1; + } /* get a work item and queue up a write request */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) { return -1; + } work->func = dm_write_func; work->write_params.item = item; @@ -544,12 +575,14 @@ dm_read(dm_item_t item, unsigned char index, void *buf, size_t count) work_q_item_t *work; /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return -1; + } /* get a work item and queue up a read request */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) { return -1; + } work->func = dm_read_func; work->read_params.item = item; @@ -567,12 +600,14 @@ dm_clear(dm_item_t item) work_q_item_t *work; /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return -1; + } /* get a work item and queue up a clear request */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) { return -1; + } work->func = dm_clear_func; work->clear_params.item = item; @@ -585,12 +620,16 @@ __EXPORT void dm_lock(dm_item_t item) { /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return; - if (item >= DM_KEY_NUM_KEYS) + } + + if (item >= DM_KEY_NUM_KEYS) { return; + } + if (g_item_locks[item]) { - sem_wait(g_item_locks[item]); + px4_sem_wait(g_item_locks[item]); } } @@ -598,12 +637,16 @@ __EXPORT void dm_unlock(dm_item_t item) { /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return; - if (item >= DM_KEY_NUM_KEYS) + } + + if (item >= DM_KEY_NUM_KEYS) { return; + } + if (g_item_locks[item]) { - sem_post(g_item_locks[item]); + px4_sem_post(g_item_locks[item]); } } @@ -614,12 +657,14 @@ dm_restart(dm_reset_reason reason) work_q_item_t *work; /* Make sure data manager has been started and is not shutting down */ - if ((g_fd < 0) || g_task_should_exit) + if ((g_fd < 0) || g_task_should_exit) { return -1; + } /* get a work item and queue up a restart request */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) { return -1; + } work->func = dm_restart_func; work->restart_params.reason = reason; @@ -636,18 +681,23 @@ task_main(int argc, char *argv[]) /* Initialize global variables */ g_key_offsets[0] = 0; - for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++) + for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++) { g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size); + } unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size); - for (unsigned i = 0; i < dm_number_of_funcs; i++) + for (unsigned i = 0; i < dm_number_of_funcs; i++) { g_func_counts[i] = 0; + } /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */ - sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ - for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) + px4_sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ + + for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) { g_item_locks[i] = NULL; + } + g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex; g_task_should_exit = false; @@ -655,40 +705,39 @@ task_main(int argc, char *argv[]) init_q(&g_work_q); init_q(&g_free_q); - sem_init(&g_work_queued_sema, 1, 0); + px4_sem_init(&g_work_queued_sema, 1, 0); /* See if the data manage file exists and is a multiple of the sector size */ g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY); + if (g_task_fd >= 0) { /* File exists, check its size */ int file_size = lseek(g_task_fd, 0, SEEK_END); + if ((file_size % k_sector_size) != 0) { warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path); + warnx("Size: %u, sector size: %d", file_size, k_sector_size); close(g_task_fd); unlink(k_data_manager_device_path); + } else { close(g_task_fd); } } /* Open or create the data manager file */ - g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY -#ifdef __PX4_LINUX - // Open with read/write permission for user - , S_IRUSR | S_IWUSR -#endif - ); + g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666); if (g_task_fd < 0) { warnx("Could not open data manager file %s", k_data_manager_device_path); - sem_post(&g_init_sema); /* Don't want to hang startup */ + px4_sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { close(g_task_fd); warnx("Could not seek data manager file %s", k_data_manager_device_path); - sem_post(&g_init_sema); /* Don't want to hang startup */ + px4_sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } @@ -697,16 +746,20 @@ task_main(int argc, char *argv[]) printf("dataman: "); /* see if we need to erase any items based on restart type */ int sys_restart_val; + if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) { if (sys_restart_val == DM_INIT_REASON_POWER_ON) { printf("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); + } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { printf("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); + } else { printf("Unknown restart"); } + } else { printf("Unknown restart"); } @@ -719,7 +772,7 @@ task_main(int argc, char *argv[]) printf(", data manager file '%s' size is %d bytes\n", k_data_manager_device_path, max_offset); /* Tell startup that the worker thread has completed its initialization */ - sem_post(&g_init_sema); + px4_sem_post(&g_init_sema); /* Start the endless loop, waiting for then processing work requests */ while (true) { @@ -732,7 +785,7 @@ task_main(int argc, char *argv[]) if (!g_task_should_exit) { /* wait for work */ - sem_wait(&g_work_queued_sema); + px4_sem_wait(&g_work_queued_sema); } /* Empty the work queue */ @@ -743,7 +796,8 @@ task_main(int argc, char *argv[]) case dm_write_func: g_func_counts[dm_write_func]++; work->result = - _write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, work->write_params.count); + _write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, + work->write_params.count); break; case dm_read_func: @@ -768,12 +822,13 @@ task_main(int argc, char *argv[]) } /* Inform the caller that work is done */ - sem_post(&work->wait_sem); + px4_sem_post(&work->wait_sem); } /* time to go???? */ - if ((g_task_should_exit) && (g_fd < 0)) + if ((g_task_should_exit) && (g_fd < 0)) { break; + } } close(g_task_fd); @@ -781,16 +836,19 @@ task_main(int argc, char *argv[]) /* The work queue is now empty, empty the free queue */ for (;;) { - if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) + if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) { break; - if (work->first) + } + + if (work->first) { free(work); + } } destroy_q(&g_work_q); destroy_q(&g_free_q); - sem_destroy(&g_work_queued_sema); - sem_destroy(&g_sys_state_mutex); + px4_sem_destroy(&g_work_queued_sema); + px4_sem_destroy(&g_sys_state_mutex); return 0; } @@ -800,17 +858,17 @@ start(void) { int task; - sem_init(&g_init_sema, 1, 0); + px4_sem_init(&g_init_sema, 1, 0); - /* start the worker thread */ - if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) { + /* start the worker thread with low priority for disk IO */ + if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 10, 1200, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } /* wait for the thread to actually initialize */ - sem_wait(&g_init_sema); - sem_destroy(&g_init_sema); + px4_sem_wait(&g_init_sema); + px4_sem_destroy(&g_init_sema); return 0; } @@ -831,7 +889,7 @@ stop(void) { /* Tell the worker task to shut down */ g_task_should_exit = true; - sem_post(&g_work_queued_sema); + px4_sem_post(&g_work_queued_sema); } static void @@ -854,11 +912,12 @@ dataman_main(int argc, char *argv[]) warnx("dataman already running"); return -1; } - if (argc == 4 && strcmp(argv[2],"-f") == 0) { + + if (argc == 4 && strcmp(argv[2], "-f") == 0) { k_data_manager_device_path = strdup(argv[3]); warnx("dataman file set to: %s\n", k_data_manager_device_path); - } - else { + + } else { k_data_manager_device_path = strdup(default_device_path); } @@ -885,14 +944,17 @@ dataman_main(int argc, char *argv[]) stop(); free(k_data_manager_device_path); k_data_manager_device_path = NULL; - } - else if (!strcmp(argv[1], "status")) + + } else if (!strcmp(argv[1], "status")) { status(); - else if (!strcmp(argv[1], "poweronrestart")) + + } else if (!strcmp(argv[1], "poweronrestart")) { dm_restart(DM_INIT_REASON_POWER_ON); - else if (!strcmp(argv[1], "inflightrestart")) + + } else if (!strcmp(argv[1], "inflightrestart")) { dm_restart(DM_INIT_REASON_IN_FLIGHT); - else { + + } else { usage(); return -1; } diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index ad8df4970b86..a29a29c735c8 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -39,6 +39,7 @@ #ifndef _DATAMANAGER_H #define _DATAMANAGER_H +#include #include #include @@ -46,92 +47,92 @@ extern "C" { #endif - /** Types of items that the data manager can store */ - typedef enum { - DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ - DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ - DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ - DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ - DM_KEY_MISSION_STATE, /* Persistent mission state */ - DM_KEY_NUM_KEYS /* Total number of item types defined */ - } dm_item_t; - - #define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1) - - /** The maximum number of instances for each item type */ - enum { - DM_KEY_SAFE_POINTS_MAX = 8, - #ifdef __cplusplus - DM_KEY_FENCE_POINTS_MAX = fence_s::GEOFENCE_MAX_VERTICES, - #else - DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - #endif - DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_MISSION_STATE_MAX = 1 - }; - - /** Data persistence levels */ - typedef enum { - DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ - DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ - DM_PERSIST_VOLATILE /* Data does not survive resets */ - } dm_persitence_t; - - /** The reason for the last reset */ - typedef enum { - DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ - DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ - DM_INIT_REASON_VOLATILE /* Data does not survive reset */ - } dm_reset_reason; - - /** Maximum size in bytes of a single item instance */ - #define DM_MAX_DATA_SIZE 124 - - /** Retrieve from the data manager store */ - __EXPORT ssize_t - dm_read( - dm_item_t item, /* The item type to retrieve */ - unsigned char index, /* The index of the item */ - void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ - ); - - /** write to the data manager store */ - __EXPORT ssize_t - dm_write( - dm_item_t item, /* The item type to store */ - unsigned char index, /* The index of the item */ - dm_persitence_t persistence, /* The persistence level of this item */ - const void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ - ); - - /** Lock all items of this type */ - __EXPORT void - dm_lock( - dm_item_t item /* The item type to clear */ - ); - - /** Unlock all items of this type */ - __EXPORT void - dm_unlock( - dm_item_t item /* The item type to clear */ - ); - - /** Erase all items of this type */ - __EXPORT int - dm_clear( - dm_item_t item /* The item type to clear */ - ); - - /** Tell the data manager about the type of the last reset */ - __EXPORT int - dm_restart( - dm_reset_reason restart_type /* The last reset type */ - ); +/** Types of items that the data manager can store */ +typedef enum { + DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ + DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ + DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ + DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ + DM_KEY_NUM_KEYS /* Total number of item types defined */ +} dm_item_t; + +#define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1) + +/** The maximum number of instances for each item type */ +enum { + DM_KEY_SAFE_POINTS_MAX = 8, +#ifdef __cplusplus + DM_KEY_FENCE_POINTS_MAX = fence_s::GEOFENCE_MAX_VERTICES, +#else + DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, +#endif + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_MISSION_STATE_MAX = 1 +}; + +/** Data persistence levels */ +typedef enum { + DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ + DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ + DM_PERSIST_VOLATILE /* Data does not survive resets */ +} dm_persitence_t; + +/** The reason for the last reset */ +typedef enum { + DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ + DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ + DM_INIT_REASON_VOLATILE /* Data does not survive reset */ +} dm_reset_reason; + +/** Maximum size in bytes of a single item instance */ +#define DM_MAX_DATA_SIZE 124 + +/** Retrieve from the data manager store */ +__EXPORT ssize_t +dm_read( + dm_item_t item, /* The item type to retrieve */ + unsigned char index, /* The index of the item */ + void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ +); + +/** write to the data manager store */ +__EXPORT ssize_t +dm_write( + dm_item_t item, /* The item type to store */ + unsigned char index, /* The index of the item */ + dm_persitence_t persistence, /* The persistence level of this item */ + const void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ +); + +/** Lock all items of this type */ +__EXPORT void +dm_lock( + dm_item_t item /* The item type to clear */ +); + +/** Unlock all items of this type */ +__EXPORT void +dm_unlock( + dm_item_t item /* The item type to clear */ +); + +/** Erase all items of this type */ +__EXPORT int +dm_clear( + dm_item_t item /* The item type to clear */ +); + +/** Tell the data manager about the type of the last reset */ +__EXPORT int +dm_restart( + dm_reset_reason restart_type /* The last reset type */ +); #ifdef __cplusplus } diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index ee6fcb3ac558..fc11e0f50513 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -49,10 +49,10 @@ #include #include #include +#include #include #include #include -#include #include #include #include @@ -66,13 +66,21 @@ #include #include +#include + #include +#include #include +#include +#include "estimator_22states.h" + +#include +#include //Forward declaration class AttPosEKF; -class AttitudePositionEstimatorEKF +class AttitudePositionEstimatorEKF : public control::SuperBlock { public: /** @@ -127,6 +135,8 @@ class AttitudePositionEstimatorEKF */ int set_debuglevel(unsigned debug) { _debug = debug; return 0; } + static constexpr unsigned MAX_PREDICITION_STEPS = 3; /**< maximum number of prediction steps between updates */ + private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ @@ -146,12 +156,14 @@ class AttitudePositionEstimatorEKF int _armedSub; orb_advert_t _att_pub; /**< vehicle attitude */ + orb_advert_t _ctrl_state_pub; /**< control state */ orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _local_pos_pub; /**< position in local frame */ orb_advert_t _estimator_status_pub; /**< status of the estimator */ orb_advert_t _wind_pub; /**< wind estimate */ struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct gyro_report _gyro; struct accel_report _accel; struct mag_report _mag; @@ -166,17 +178,20 @@ class AttitudePositionEstimatorEKF struct vehicle_land_detected_s _landDetector; struct actuator_armed_s _armed; - struct gyro_scale _gyro_offsets[3]; - struct accel_scale _accel_offsets[3]; - struct mag_scale _mag_offsets[3]; + hrt_abstime _last_accel; + hrt_abstime _last_mag; + unsigned _prediction_steps; + uint64_t _prediction_last; struct sensor_combined_s _sensor_combined; struct map_projection_reference_s _pos_ref; - float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ + float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; + float _vibration_warning_threshold = 1.0f; + hrt_abstime _vibration_warning_timestamp = 0; perf_counter_t _loop_perf; ///< loop performance counter perf_counter_t _loop_intvl; ///< loop rate counter @@ -193,18 +208,19 @@ class AttitudePositionEstimatorEKF bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received bool _baro_init; - float _baroAltRef; bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; - hrt_abstime _last_run; hrt_abstime _distance_last_valid; - bool _gyro_valid; - bool _accel_valid; - bool _mag_valid; + DataValidatorGroup _voter_gyro; + DataValidatorGroup _voter_accel; + DataValidatorGroup _voter_mag; int _gyro_main; ///< index of the main gyroscope int _accel_main; ///< index of the main accelerometer int _mag_main; ///< index of the main magnetometer + bool _data_good; ///< all required filter data is ok + bool _failsafe; ///< failsafe on one of the sensors + bool _vibration_warning; ///< high vibration levels detected bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 0 @@ -215,6 +231,10 @@ class AttitudePositionEstimatorEKF int _mavlink_fd; + control::BlockParamFloat _mag_offset_x; + control::BlockParamFloat _mag_offset_y; + control::BlockParamFloat _mag_offset_z; + struct { int32_t vel_delay_ms; int32_t pos_delay_ms; @@ -259,6 +279,13 @@ class AttitudePositionEstimatorEKF AttPosEKF *_ekf; + TerrainEstimator *_terrain_estimator; + + /* Low pass filter for attitude rates */ + math::LowPassFilter2p _LP_att_P; + math::LowPassFilter2p _LP_att_Q; + math::LowPassFilter2p _LP_att_R; + private: /** * Update our local parameter cache. @@ -299,6 +326,12 @@ class AttitudePositionEstimatorEKF **/ void publishAttitude(); + /** + * @brief + * Publish the system state for control modules + **/ + void publishControlState(); + /** * @brief * Publish local position relative to reference point where filter was initialized @@ -333,6 +366,12 @@ class AttitudePositionEstimatorEKF **/ void initializeGPS(); + /** + * Initialize the reference position for the local coordinate frame + */ + void initReferencePosition(hrt_abstime timestamp, bool gps_valid, + double lat, double lon, float gps_alt, float baro_alt); + /** * @brief * Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate diff --git a/src/modules/ekf_att_pos_estimator/CMakeLists.txt b/src/modules/ekf_att_pos_estimator/CMakeLists.txt new file mode 100644 index 000000000000..13eaeb6d2bfc --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/CMakeLists.txt @@ -0,0 +1,51 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +set(MODULE_CFLAGS ) +if(${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=3400) +endif() + +px4_add_module( + MODULE modules__ekf_att_pos_estimator + MAIN ekf_att_pos_estimator + STACK 3000 + COMPILE_FLAGS ${MODULE_CFLAGS} + SRCS + ekf_att_pos_estimator_main.cpp + estimator_22states.cpp + estimator_utilities.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : 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 3eea6237764a..44c7b3e4347e 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 @@ -68,13 +68,17 @@ #include #include -static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; //Constants static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds -static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds -static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure +static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds +static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure +static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) +static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) +static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) +static constexpr float EPH_LARGE_VALUE = 1000.0f; +static constexpr float EPV_LARGE_VALUE = 1000.0f; /** * estimator app start / stop handling function @@ -83,13 +87,18 @@ static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we si */ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); -__EXPORT uint32_t millis(); - -__EXPORT uint64_t getMicros(); +uint32_t millis(); +uint64_t getMicros(); +uint32_t getMillis(); uint32_t millis() { - return IMUmsec; + return getMillis(); +} + +uint32_t getMillis() +{ + return getMicros() / 1000; } uint64_t getMicros() @@ -100,16 +109,11 @@ uint64_t getMicros() namespace estimator { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - AttitudePositionEstimatorEKF *g_estimator = nullptr; } AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : + SuperBlock(NULL, "PE"), _task_should_exit(false), _task_running(false), _estimator_task(-1), @@ -128,81 +132,95 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _landDetectorSub(-1), _armedSub(-1), -/* publications */ + /* publications */ _att_pub(nullptr), + _ctrl_state_pub(nullptr), _global_pos_pub(nullptr), _local_pos_pub(nullptr), _estimator_status_pub(nullptr), _wind_pub(nullptr), - _att({}), - _gyro({}), - _accel({}), - _mag({}), - _airspeed({}), - _baro({}), - _vstatus({}), - _global_pos({}), - _local_pos({}), - _gps({}), - _wind({}), - _distance {}, - _landDetector {}, - _armed {}, - - _gyro_offsets({}), - _accel_offsets({}), - _mag_offsets({}), - - _sensor_combined {}, - - _pos_ref{}, - _baro_ref_offset(0.0f), - _baro_gps_offset(0.0f), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), - _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), - _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), - _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), - _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), - _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), - _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), - _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), - - /* states */ - _gps_alt_filt(0.0f), - _baro_alt_filt(0.0f), - _covariancePredictionDt(0.0f), - _gpsIsGood(false), - _previousGPSTimestamp(0), - _baro_init(false), - _baroAltRef(0.0f), - _gps_initialized(false), - _filter_start_time(0), - _last_sensor_timestamp(0), - _last_run(0), - _distance_last_valid(0), - _gyro_valid(false), - _accel_valid(false), - _mag_valid(false), - _gyro_main(0), - _accel_main(0), - _mag_main(0), - _ekf_logging(true), - _debug(0), - - _newHgtData(false), - _newAdsData(false), - _newDataMag(false), - _newRangeData(false), - - _mavlink_fd(-1), - _parameters {}, - _parameter_handles {}, - _ekf(nullptr) + _att{}, + _ctrl_state{}, + _gyro{}, + _accel{}, + _mag{}, + _airspeed{}, + _baro{}, + _vstatus{}, + _global_pos{}, + _local_pos{}, + _gps{}, + _wind{}, + _distance{}, + _landDetector{}, + _armed{}, + + _last_accel(0), + _last_mag(0), + _prediction_steps(0), + _prediction_last(0), + + _sensor_combined{}, + + _pos_ref{}, + _filter_ref_offset(0.0f), + _baro_gps_offset(0.0f), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), + _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), + _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), + _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), + + /* states */ + _gps_alt_filt(0.0f), + _baro_alt_filt(0.0f), + _covariancePredictionDt(0.0f), + _gpsIsGood(false), + _previousGPSTimestamp(0), + _baro_init(false), + _gps_initialized(false), + _filter_start_time(0), + _last_sensor_timestamp(hrt_absolute_time()), + _distance_last_valid(0), + _voter_gyro(3), + _voter_accel(3), + _voter_mag(3), + _gyro_main(-1), + _accel_main(-1), + _mag_main(-1), + _data_good(false), + _failsafe(false), + _vibration_warning(false), + _ekf_logging(true), + _debug(0), + + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), + + _mavlink_fd(-1), + _mag_offset_x(this, "MAGB_X"), + _mag_offset_y(this, "MAGB_Y"), + _mag_offset_z(this, "MAGB_Z"), + _parameters{}, + _parameter_handles{}, + _ekf(nullptr), + _terrain_estimator(nullptr), + + _LP_att_P(250.0f, 20.0f), + _LP_att_Q(250.0f, 20.0f), + _LP_att_R(250.0f, 20.0f) { - _last_run = hrt_absolute_time(); + _voter_mag.set_timeout(200000); + + _terrain_estimator = new TerrainEstimator(); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); @@ -223,50 +241,12 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); _parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT"); + /* indicate consumers that the current position data is not valid */ + _gps.eph = 10000.0f; + _gps.epv = 10000.0f; + /* fetch initial parameter values */ parameters_update(); - - /* get offsets */ - int fd, res; - - for (unsigned s = 0; s < 3; s++) { - char str[30]; - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - fd = px4_open(str, O_RDONLY); - - if (fd >= 0) { - res = px4_ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]); - close(fd); - - if (res) { - warnx("G%u SCALE FAIL", s); - } - } - - (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); - fd = px4_open(str, O_RDONLY); - - if (fd >= 0) { - res = px4_ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets[s]); - px4_close(fd); - - if (res) { - warnx("A%u SCALE FAIL", s); - } - } - - (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); - fd = px4_open(str, O_RDONLY); - - if (fd >= 0) { - res = px4_ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets[s]); - px4_close(fd); - - if (res) { - warnx("M%u SCALE FAIL", s); - } - } - } } AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() @@ -290,7 +270,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() } } while (_estimator_task != -1); } - + delete _terrain_estimator; delete _ekf; estimator::g_estimator = nullptr; @@ -305,6 +285,8 @@ int AttitudePositionEstimatorEKF::enable_logging(bool logging) int AttitudePositionEstimatorEKF::parameters_update() { + /* update C++ param system */ + updateParams(); param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms)); @@ -342,6 +324,13 @@ int AttitudePositionEstimatorEKF::parameters_update() _ekf->accelProcessNoise = _parameters.acc_pnoise; _ekf->airspeedMeasurementSigma = _parameters.eas_noise; _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF + #if 0 + // Initially disable loading until + // convergence is flight-test proven + _ekf->magBias.x = _mag_offset_x.get(); + _ekf->magBias.y = _mag_offset_y.get(); + _ekf->magBias.z = _mag_offset_z.get(); + #endif } return OK; @@ -354,12 +343,24 @@ void AttitudePositionEstimatorEKF::vehicle_status_poll() /* Check HIL state if vehicle status has changed */ orb_check(_vstatus_sub, &vstatus_updated); + bool landed = _vstatus.condition_landed; + if (vstatus_updated) { orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); - //Tell EKF that the vehicle is a fixed wing or multi-rotor + // Tell EKF that the vehicle is a fixed wing or multi-rotor _ekf->setIsFixedWing(!_vstatus.is_rotary_wing); + + // Save params on landed + if (!landed && _vstatus.condition_landed) { + _mag_offset_x.set(_ekf->magBias.x); + _mag_offset_x.commit(); + _mag_offset_y.set(_ekf->magBias.y); + _mag_offset_y.commit(); + _mag_offset_z.set(_ekf->magBias.z); + _mag_offset_z.commit(); + } } } @@ -394,7 +395,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Do not warn about accel offset if we have no position updates if (!(warn_index == 5 && _ekf->staticMode)) { - warnx("reset: %s", feedback[warn_index]); + PX4_WARN("reset: %s", feedback[warn_index]); mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); } } @@ -408,6 +409,15 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Count the reset condition perf_count(_perf_reset); + // GPS is in scaled integers, convert + double lat = _gps.lat / 1.0e7; + double lon = _gps.lon / 1.0e7; + float gps_alt = _gps.alt / 1e3f; + + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -442,7 +452,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() if (_debug > 10) { if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { - warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", + PX4_INFO("health: VEL:%s POS:%s HGT:%s OFFS:%s", ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), @@ -450,7 +460,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() } if (rep.timeout_flags) { - warnx("timeout: %s%s%s%s", + PX4_INFO("timeout: %s%s%s%s", ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), @@ -463,8 +473,13 @@ int AttitudePositionEstimatorEKF::check_filter_state() size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; + // Copy diagonal elemnts of covariance matrix + float covariances[28]; + _ekf->get_covariance(covariances); + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; + rep.covariances[i] = covariances[i]; } @@ -487,17 +502,17 @@ void AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[]) void AttitudePositionEstimatorEKF::task_main() { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); - _filter_start_time = hrt_absolute_time(); - if (!_ekf) { - warnx("OUT OF MEM!"); + PX4_ERR("OUT OF MEM!"); return; } + _filter_start_time = hrt_absolute_time(); + /* * do subscriptions */ @@ -515,8 +530,6 @@ void AttitudePositionEstimatorEKF::task_main() orb_set_interval(_vstatus_sub, 200); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_sensor_combined_sub, 9); /* sets also parameters in the EKF object */ parameters_update(); @@ -580,18 +593,23 @@ void AttitudePositionEstimatorEKF::task_main() /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); + /* HIL is slow, set permissive timeouts */ + _voter_gyro.set_timeout(500000); + _voter_accel.set_timeout(500000); + _voter_mag.set_timeout(500000); + /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); /* set sensors to de-initialized state */ - _gyro_valid = false; - _accel_valid = false; - _mag_valid = false; + _gyro_main = -1; + _accel_main = -1; + _mag_main = -1; _baro_init = false; _gps_initialized = false; + _last_sensor_timestamp = hrt_absolute_time(); - _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; @@ -619,19 +637,22 @@ void AttitudePositionEstimatorEKF::task_main() * We run the filter only once all data has been fetched **/ - if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) { // maintain filtered baro and gps altitudes to calculate weather offset // baro sample rate is ~70Hz and measurement bandwidth is high // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds // maintain heavily filtered values for both baro and gps altitude // Assume the filtered output should be identical for both sensors - _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; + + if (_gpsIsGood) { + _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; + } // if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { // _last_debug_print = hrt_absolute_time(); // perf_print_counter(_perf_baro); // perf_reset(_perf_baro); -// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", +// PX4_INFO("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", // (double)_baro_gps_offset, // (double)_baro_alt_filt, // (double)_gps_alt_filt, @@ -648,12 +669,15 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->posNE[1] = 0.0f; _local_pos.ref_alt = 0.0f; - _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + _filter_ref_offset = -_baro.altitude; + + PX4_INFO("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); + } else { if (!_gps_initialized && _gpsIsGood) { @@ -673,21 +697,27 @@ void AttitudePositionEstimatorEKF::task_main() continue; } - //Run EKF data fusion steps + // Run EKF data fusion steps updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); - //Publish attitude estimations + // Run separate terrain estimator + _terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance); + _terrain_estimator->measurement_update(hrt_absolute_time(), &_gps, &_distance, &_att); + + // Publish attitude estimations publishAttitude(); - //Publish Local Position estimations + // publish control state + publishControlState(); + + // Publish Local Position estimations publishLocalPosition(); - //Publish Global Position, but only if it's any good - if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) { - publishGlobalPosition(); - } + // Publish Global Position, it will have a large uncertainty + // set if only altitude is known + publishGlobalPosition(); - //Publish wind estimates + // Publish wind estimates if (hrt_elapsed_time(&_wind.timestamp) > 99000) { publishWindEstimate(); } @@ -705,6 +735,35 @@ void AttitudePositionEstimatorEKF::task_main() return; } +void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, + bool gps_valid, double lat, double lon, float gps_alt, float baro_alt) +{ + // Reference altitude + if (PX4_ISFINITE(_ekf->states[9])) { + _filter_ref_offset = _ekf->states[9]; + } else if (PX4_ISFINITE(-_ekf->hgtMea)) { + _filter_ref_offset = -_ekf->hgtMea; + } else { + _filter_ref_offset = -_baro.altitude; + } + + // init filtered gps and baro altitudes + _baro_alt_filt = baro_alt; + + if (gps_valid) { + _gps_alt_filt = gps_alt; + + // Initialize projection + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; + _local_pos.ref_alt = gps_alt; + _local_pos.ref_timestamp = timestamp; + + map_projection_init(&_pos_ref, lat, lon); + mavlink_and_console_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + } +} + void AttitudePositionEstimatorEKF::initializeGPS() { // GPS is in scaled integers, convert @@ -714,11 +773,6 @@ void AttitudePositionEstimatorEKF::initializeGPS() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame - - // init filtered gps and baro altitudes - _gps_alt_filt = gps_alt; - _baro_alt_filt = _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = _ekf->baroHgt; @@ -740,21 +794,14 @@ void AttitudePositionEstimatorEKF::initializeGPS() _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - // Initialize projection - _local_pos.ref_lat = lat; - _local_pos.ref_lon = lon; - _local_pos.ref_alt = gps_alt; - _local_pos.ref_timestamp = _gps.timestamp_position; - - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); #if 0 - warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, + PX4_INFO("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, - (double)_baro_ref_offset); - warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, + PX4_INFO("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, + (double)_filter_ref_offset); + PX4_INFO("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif @@ -787,9 +834,9 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt; + _att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; @@ -798,7 +845,7 @@ void AttitudePositionEstimatorEKF::publishAttitude() /* lazily publish the attitude only once available */ if (_att_pub != nullptr) { - /* publish the attitude setpoint */ + /* publish the attitude */ orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); } else { @@ -807,6 +854,78 @@ void AttitudePositionEstimatorEKF::publishAttitude() } } +void AttitudePositionEstimatorEKF::publishControlState() +{ + /* Accelerations in Body Frame */ + _ctrl_state.x_acc = _ekf->accel.x; + _ctrl_state.y_acc = _ekf->accel.y; + _ctrl_state.z_acc = _ekf->accel.z; + + /* Velocity in Body Frame */ + Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]); + Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]); + Vector3f v_b = _ekf->Tnb * v_n; + Vector3f v_b_var = _ekf->Tnb * v_n_var; + + _ctrl_state.x_vel = v_b.x; + _ctrl_state.y_vel = v_b.y; + _ctrl_state.z_vel = v_b.z; + + _ctrl_state.vel_variance[0] = v_b_var.x; + _ctrl_state.vel_variance[1] = v_b_var.y; + _ctrl_state.vel_variance[2] = v_b_var.z; + + /* Local Position */ + _ctrl_state.x_pos = _ekf->states[7]; + _ctrl_state.y_pos = _ekf->states[8]; + + // XXX need to announce change of Z reference somehow elegantly + _ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset; + + _ctrl_state.pos_variance[0] = _ekf->P[7][7]; + _ctrl_state.pos_variance[1] = _ekf->P[8][8]; + _ctrl_state.pos_variance[2] = _ekf->P[9][9]; + + /* Attitude */ + _ctrl_state.timestamp = _last_sensor_timestamp; + _ctrl_state.q[0] = _ekf->states[0]; + _ctrl_state.q[1] = _ekf->states[1]; + _ctrl_state.q[2] = _ekf->states[2]; + _ctrl_state.q[3] = _ekf->states[3]; + + /* Airspeed (Groundspeed - Windspeed) */ + //_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); + // the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL + // and in outdoor tests + _ctrl_state.airspeed = _airspeed.true_airspeed_m_s; + /* Attitude Rates */ + _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; + _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; + _ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt; + + /* Guard from bad data */ + if (!PX4_ISFINITE(_ctrl_state.x_vel) || + !PX4_ISFINITE(_ctrl_state.y_vel) || + !PX4_ISFINITE(_ctrl_state.z_vel) || + !PX4_ISFINITE(_ctrl_state.x_pos) || + !PX4_ISFINITE(_ctrl_state.y_pos) || + !PX4_ISFINITE(_ctrl_state.z_pos)) + { + // bad data, abort publication + return; + } + + /* lazily publish the control state only once available */ + if (_ctrl_state_pub != nullptr) { + /* publish the control state */ + orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state); + + } else { + /* advertise and publish */ + _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state); + } +} + void AttitudePositionEstimatorEKF::publishLocalPosition() { _local_pos.timestamp = _last_sensor_timestamp; @@ -814,7 +933,8 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef; + _local_pos.z = _ekf->states[9] - _filter_ref_offset; + //_local_pos.z_stable = _ekf->states[9]; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; @@ -829,6 +949,17 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.z_global = false; _local_pos.yaw = _att.yaw; + if (!PX4_ISFINITE(_local_pos.x) || + !PX4_ISFINITE(_local_pos.y) || + !PX4_ISFINITE(_local_pos.z) || + !PX4_ISFINITE(_local_pos.vx) || + !PX4_ISFINITE(_local_pos.vy) || + !PX4_ISFINITE(_local_pos.vz)) + { + // bad data, abort publication + return; + } + /* lazily publish the local position only once available */ if (_local_pos_pub != nullptr) { /* publish the attitude setpoint */ @@ -850,6 +981,10 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_utc_usec = _gps.time_utc_usec; + } else { + _global_pos.lat = 0.0; + _global_pos.lon = 0.0; + _global_pos.time_utc_usec = 0; } if (_local_pos.v_xy_valid) { @@ -862,20 +997,49 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = (-_local_pos.z) - _baro_gps_offset; + _global_pos.alt = (-_local_pos.z) - _filter_ref_offset - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; + } else { + _global_pos.vel_d = 0.0f; } /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); + if (_terrain_estimator->is_valid()) { + _global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground(); + _global_pos.terrain_alt_valid = true; + } else { + _global_pos.terrain_alt_valid = false; + } _global_pos.yaw = _local_pos.yaw; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + + const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; + + if (!_local_pos.xy_global || + !_local_pos.v_xy_valid || + _gps.timestamp_position == 0 || + (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + + _global_pos.eph = EPH_LARGE_VALUE; + _global_pos.epv = EPV_LARGE_VALUE; + + } else { + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; + } + + if (!PX4_ISFINITE(_global_pos.lat) || + !PX4_ISFINITE(_global_pos.lon) || + !PX4_ISFINITE(_global_pos.alt) || + !PX4_ISFINITE(_global_pos.vel_n) || + !PX4_ISFINITE(_global_pos.vel_e) || + !PX4_ISFINITE(_global_pos.vel_d)) + { + // bad data, abort publication + return; + } /* lazily publish the global position only once available */ if (_global_pos_pub != nullptr) { @@ -918,13 +1082,22 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->UpdateStrapdownEquationsNED(); // store the predicted states for subsequent use by measurement fusion - _ekf->StoreStates(IMUmsec); + _ekf->StoreStates(getMillis()); // sum delta angles and time used by covariance prediction _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; _covariancePredictionDt += _ekf->dtIMU; + // only fuse every few steps + if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) { + _prediction_steps++; + return; + } else { + _prediction_steps = 0; + _prediction_last = hrt_absolute_time(); + } + // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) @@ -944,8 +1117,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + _ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -968,8 +1141,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + _ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -985,7 +1158,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + _ekf->RecallStates(_ekf->statesAtHgtTime, (getMillis() - _parameters.height_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -998,7 +1171,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseMag) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, - (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + (getMillis() - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data _ekf->magstate.obsIndex = 0; _ekf->FuseMagnetometer(); @@ -1010,10 +1183,10 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const } // Fuse Airspeed Measurements - if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) { + if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, - (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + (getMillis() - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); } else { @@ -1026,7 +1199,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // _ekf->rngMea is set in sensor readout already _ekf->fuseRngData = true; _ekf->fuseOptFlowData = false; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->RecallStates(_ekf->statesAtRngTime, (getMillis() - 100.0f)); _ekf->OpticalFlowEKF(); _ekf->fuseRngData = false; } @@ -1040,8 +1213,8 @@ int AttitudePositionEstimatorEKF::start() /* start the task */ _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 7500, + SCHED_PRIORITY_MAX - 20, + 4600, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); @@ -1059,7 +1232,7 @@ void AttitudePositionEstimatorEKF::print_status() math::Matrix<3, 3> R = q.to_dcm(); math::Vector<3> euler = R.to_euler(); - printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", + PX4_INFO("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); // State vector: @@ -1072,42 +1245,45 @@ void AttitudePositionEstimatorEKF::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); - printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset, + PX4_INFO("dtIMU: %8.6f filt: %8.6f IMU (ms): %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)getMillis()); + PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt); + PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt); + PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset, (double)_baro_gps_offset); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, + PX4_INFO("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, + PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], + + PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], + PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], + PX4_INFO("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], + PX4_INFO("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); if (EKF_STATE_ESTIMATES == 23) { - printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], + PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]); + PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]); + PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], + PX4_INFO("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); - printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); + PX4_INFO("states (terrain) [22]: %8.4f", (double)_ekf->states[22]); } else { - printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); - printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], - (double)_ekf->states[17]); - printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], - (double)_ekf->states[20]); + PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]); + PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]); + PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17], + (double)_ekf->states[18]); + PX4_INFO("states (mag bias) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20], + (double)_ekf->states[21]); } - printf("states: %s %s %s %s %s %s %s %s %s %s\n", + PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s", (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", @@ -1118,6 +1294,13 @@ void AttitudePositionEstimatorEKF::print_status() (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + + PX4_INFO("gyro status:"); + _voter_gyro.print(); + PX4_INFO("accel status:"); + _voter_accel.print(); + PX4_INFO("mag status:"); + _voter_mag.print(); } void AttitudePositionEstimatorEKF::pollData() @@ -1130,127 +1313,181 @@ void AttitudePositionEstimatorEKF::pollData() orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed); } - //Update Gyro and Accelerometer - static Vector3f lastAngRate; - static Vector3f lastAccel; - bool accel_updated = false; - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); - static hrt_abstime last_accel = 0; - static hrt_abstime last_mag = 0; + /* set time fields */ + IMUusec = _sensor_combined.timestamp; + float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f; - if (last_accel != _sensor_combined.accelerometer_timestamp) { - accel_updated = true; + /* guard against too large deltaT's */ + if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.0001f) { - } else { - accel_updated = false; + if (PX4_ISFINITE(_ekf->dtIMUfilt) && _ekf->dtIMUfilt < 0.5f && _ekf->dtIMUfilt > 0.0001f) { + deltaT = _ekf->dtIMUfilt; + } else { + deltaT = 0.01f; + } } - last_accel = _sensor_combined.accelerometer_timestamp; + /* fill in last data set */ + _ekf->dtIMU = deltaT; + // Feed validator with recent sensor data - // Copy gyro and accel - _last_sensor_timestamp = _sensor_combined.timestamp; - IMUmsec = _sensor_combined.timestamp / 1e3; - IMUusec = _sensor_combined.timestamp; + for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { + _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], + _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); + _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], + _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); + _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], + _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); + } - float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; + // Get best measurement values + hrt_abstime curr_time = hrt_absolute_time(); + (void)_voter_gyro.get_best(curr_time, &_gyro_main); + if (_gyro_main >= 0) { - /* guard against too large deltaT's */ - if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { - deltaT = 0.01f; + // Use pre-integrated values if possible + if (_sensor_combined.gyro_integral_dt[_gyro_main] > 0) { + _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0]; + _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; + _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; + } else { + float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f; + if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) { + deltaT = dt_gyro; + _ekf->dtIMU = deltaT; + } + _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU; + _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU; + _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU; + } + + _ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]; + perf_count(_perf_gyro); } - _last_run = _sensor_combined.timestamp; + (void)_voter_accel.get_best(curr_time, &_accel_main); + if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) { - // Always store data, independent of init status - /* fill in last data set */ - _ekf->dtIMU = deltaT; + // Use pre-integrated values if possible + if (_sensor_combined.accelerometer_integral_dt[_accel_main] > 0) { + _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0]; + _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1]; + _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2]; + } else { + _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU; + _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU; + _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) * _ekf->dtIMU; + } - int last_gyro_main = _gyro_main; + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]; + _last_accel = _sensor_combined.accelerometer_timestamp[_accel_main]; + } - if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) && - PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) && - PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) && - (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { + (void)_voter_mag.get_best(curr_time, &_mag_main); + if (_mag_main >= 0) { + Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1], + _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]); - _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; - _gyro_main = 0; - _gyro_valid = true; + /* fail over to the 2nd mag if we know the first is down */ + if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) { + _ekf->magData.x = mag.x; + _ekf->magData.y = mag.y; + _ekf->magData.z = mag.z; + _newDataMag = true; + _last_mag = _sensor_combined.magnetometer_timestamp[_mag_main]; + perf_count(_perf_mag); + } + } - } else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) && - PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) && - PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) { + if (!_failsafe && (_voter_gyro.failover_count() > 0 || + _voter_accel.failover_count() > 0 || + _voter_mag.failover_count() > 0)) { - _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro1_rad_s[2]; - _gyro_main = 1; - _gyro_valid = true; + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } + if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { + + if (_vibration_warning_timestamp == 0) { + _vibration_warning_timestamp = curr_time; + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { + _vibration_warning = true; + mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d", + (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), + (int)(100 * _voter_accel.get_vibration_factor(curr_time)), + (int)(100 * _voter_mag.get_vibration_factor(curr_time))); + } } else { - _gyro_valid = false; + _vibration_warning_timestamp = 0; } - if (last_gyro_main != _gyro_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main); - } + _last_sensor_timestamp = _sensor_combined.timestamp; - if (!_gyro_valid) { - /* keep last value if he hit an out of band value */ - lastAngRate = _ekf->angRate; + // XXX this is for assessing the filter performance + // leave this in as long as larger improvements are still being made. + #if 0 - } else { - perf_count(_perf_gyro); - } + float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f; + float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f; - if (accel_updated) { + static unsigned dtoverflow5 = 0; + static unsigned dtoverflow10 = 0; + static hrt_abstime lastprint = 0; - int last_accel_main = _accel_main; + if (hrt_elapsed_time(&lastprint) > 1000000 || _sensor_combined.gyro_rad_s[0] > 4.0f) { + PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", + (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, + dtoverflow5, dtoverflow10); - /* fail over to the 2nd accel if we know the first is down */ - if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) { - _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; - _accel_main = 0; + PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, + (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); - } else { - _ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2]; - _accel_main = 1; - } + PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), + (double)(_sensor_combined.accelerometer_integral_m_s[0]), + (double)(_sensor_combined.accelerometer_integral_m_s[1]), + (double)(_sensor_combined.accelerometer_integral_m_s[2])); - if (!_accel_valid) { - lastAccel = _ekf->accel; - } + PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); - if (last_accel_main != _accel_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main); - } + PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f", + (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); - _accel_valid = true; - } + PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f", + (double)_sensor_combined.gyro_rad_s[0], + (double)_sensor_combined.gyro_rad_s[1], + (double)_sensor_combined.gyro_rad_s[2]); - _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; - lastAngRate = _ekf->angRate; - _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; - lastAccel = _ekf->accel; + lastprint = hrt_absolute_time(); + } - if (last_mag != _sensor_combined.magnetometer_timestamp) { - _newDataMag = true; + if (deltaT > 0.005f) { + dtoverflow5++; + } - } else { - _newDataMag = false; + if (deltaT > 0.01f) { + dtoverflow10++; } + #endif - last_mag = _sensor_combined.magnetometer_timestamp; + _data_good = true; - //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); + //PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); //Update Land Detector bool newLandData; @@ -1267,10 +1504,9 @@ void AttitudePositionEstimatorEKF::pollData() orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); - _ekf->VtasMeas = _airspeed.true_airspeed_m_s; + _ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s; } - bool gps_update; orb_check(_gps_sub, &gps_update); @@ -1286,7 +1522,7 @@ void AttitudePositionEstimatorEKF::pollData() } //Check if the GPS fix is good enough for us to use - if (_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { + if ((_gps.fix_type >= 3) && (_gps.eph < requiredAccuracy) && (_gps.epv < requiredAccuracy)) { _gpsIsGood = true; } else { @@ -1320,7 +1556,11 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD)); // update LPF - _gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + + if (PX4_ISFINITE(filter_step)) { + _gps_alt_filt += filter_step; + } } //check if we had a GPS outage for a long time @@ -1335,21 +1575,21 @@ void AttitudePositionEstimatorEKF::pollData() } } - //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); + //PX4_INFO("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { - // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); + // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); // } else { - // _ekf->vneSigma = _parameters.velne_noise; + // _ekf->vneSigma = _parameters.velne_noise; // } // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { - // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); + // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); // } else { - // _ekf->posNeSigma = _parameters.posne_noise; + // _ekf->posNeSigma = _parameters.posne_noise; // } - // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + // PX4_INFO("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); _previousGPSTimestamp = _gps.timestamp_position; @@ -1358,7 +1598,7 @@ void AttitudePositionEstimatorEKF::pollData() // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, // then something is very wrong with the GPS (possibly a hardware failure or comlink error) - const float dtLastGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; if (dtLastGoodGPS >= POS_RESET_THRESHOLD) { @@ -1403,68 +1643,21 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; - if(!_baro_init) { + if (!_baro_init) { _baro_init = true; - _baroAltRef = _baro.altitude; + _baro_alt_filt = _baro.altitude; } _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); - - perf_count(_perf_baro); - } - - //Update Magnetometer - if (_newDataMag) { - - _mag_valid = true; - - perf_count(_perf_mag); - - int last_mag_main = _mag_main; - - Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1], - _sensor_combined.magnetometer_ga[2]); + float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); - Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1], - _sensor_combined.magnetometer1_ga[2]); - - const unsigned mag_timeout_us = 200000; - - /* fail over to the 2nd mag if we know the first is down */ - if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us && - _sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount && - mag0.length() > 0.1f) { - _ekf->magData.x = mag0.x; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - - _ekf->magData.y = mag0.y; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - - _ekf->magData.z = mag0.z; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 0; - - } else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us && - mag1.length() > 0.1f) { - _ekf->magData.x = mag1.x; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - - _ekf->magData.y = mag1.y; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - - _ekf->magData.z = mag1.z; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 1; - } else { - _mag_valid = false; + if (PX4_ISFINITE(filter_step)) { + _baro_alt_filt += filter_step; } - if (last_mag_main != _mag_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main); - } + perf_count(_perf_baro); } //Update range data @@ -1490,38 +1683,42 @@ int AttitudePositionEstimatorEKF::trip_nan() // If system is not armed, inject a NaN value into the filter if (_armed.armed) { - warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); + PX4_INFO("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); ret = 1; } else { float nan_val = 0.0f / 0.0f; - warnx("system not armed, tripping state vector with NaN values"); + PX4_INFO("system not armed, tripping state vector with NaN"); _ekf->states[5] = nan_val; usleep(100000); - warnx("tripping covariance #1 with NaN values"); + PX4_INFO("tripping covariance #1 with NaN"); _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #2 with NaN values"); + PX4_INFO("tripping covariance #2 with NaN"); _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #3 with NaN values"); + PX4_INFO("tripping covariance #3 with NaN"); _ekf->P[3][3] = nan_val; // covariance matrix usleep(100000); - warnx("tripping Kalman gains with NaN values"); + PX4_INFO("tripping Kalman gains with NaN"); _ekf->Kfusion[0] = nan_val; // Kalman gains usleep(100000); - warnx("tripping stored states[0] with NaN values"); + PX4_INFO("tripping stored states[0] with NaN"); _ekf->storedStates[0][0] = nan_val; usleep(100000); - warnx("\nDONE - FILTER STATE:"); + PX4_INFO("tripping states[9] with NaN"); + _ekf->states[9] = nan_val; + usleep(100000); + + PX4_INFO("DONE - FILTER STATE:"); print_status(); } @@ -1531,45 +1728,42 @@ int AttitudePositionEstimatorEKF::trip_nan() int ekf_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 2) { - warnx("usage: ekf_att_pos_estimator {start|stop|status|logging}"); + PX4_ERR("usage: ekf_att_pos_estimator {start|stop|status|logging}"); return 1; } if (!strcmp(argv[1], "start")) { if (estimator::g_estimator != nullptr) { - warnx("already running"); + PX4_ERR("already running"); return 1; } estimator::g_estimator = new AttitudePositionEstimatorEKF(); if (estimator::g_estimator == nullptr) { - warnx("alloc failed"); + PX4_ERR("alloc failed"); return 1; } if (OK != estimator::g_estimator->start()) { delete estimator::g_estimator; estimator::g_estimator = nullptr; - warnx("start failed"); + PX4_ERR("start failed"); return 1; } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) { usleep(50000); - printf("."); - fflush(stdout); + PX4_INFO("."); } - printf("\n"); - return 0; } if (estimator::g_estimator == nullptr) { - warnx("not running"); + PX4_ERR("not running"); return 1; } @@ -1581,8 +1775,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) } if (!strcmp(argv[1], "status")) { - warnx("running"); - estimator::g_estimator->print_status(); return 0; @@ -1607,6 +1799,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) return ret; } - warnx("unrecognized command"); + PX4_ERR("unrecognized command"); return 1; } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index f45700eada90..45e2f24939c6 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -36,13 +36,9 @@ * * Parameters defined by the attitude and position estimator task * - * @author Lorenz Meier + * @author Lorenz Meier */ -#include - -#include - /* * Estimator parameters, accessible via MAVLink * @@ -141,13 +137,13 @@ PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f); /** * Velocity noise in down (vertical) direction * - * Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7 + * Generic default: 0.3, multicopters: 0.4, ground vehicles: 0.7 * - * @min 0.05 - * @max 5.0 + * @min 0.2 + * @max 3.0 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f); +PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.3f); /** * Position noise in north-east (horizontal) direction @@ -163,21 +159,21 @@ PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f); /** * Position noise in down (vertical) direction * - * Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0 + * Generic defaults: 1.25, multicopters: 1.0, ground vehicles: 1.0 * - * @min 0.1 - * @max 10.0 + * @min 0.5 + * @max 3.0 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f); +PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 1.25f); /** * Magnetometer measurement noise * * Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05 * - * @min 0.1 - * @max 10.0 + * @min 0.01 + * @max 1.0 * @group Position Estimator */ PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f); @@ -214,23 +210,23 @@ PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f. * Increasing this value will make the gyro bias converge faster but noisier. * - * @min 0.0000001 + * @min 0.00000005 * @max 0.00001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-06f); +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); /** * Accelerometer bias estimate process noise * - * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f. + * Generic defaults: 0.00001f, multicopters: 0.00001f, ground vehicles: 0.00001f. * Increasing this value makes the bias estimation faster and noisier. * * @min 0.00001 * @max 0.001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0002f); +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 1e-05f); /** * Magnetometer earth frame offsets process noise @@ -258,6 +254,42 @@ PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); */ PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); +/** + * Magnetometer X bias + * + * The magnetometer bias. This bias is learnt by the filter over time and + * persists between boots. + * + * @min -0.6 + * @max 0.6 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_X, 0.0f); + +/** + * Magnetometer Y bias + * + * The magnetometer bias. This bias is learnt by the filter over time and + * persists between boots. + * + * @min -0.6 + * @max 0.6 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_Y, 0.0f); + +/** + * Magnetometer Z bias + * + * The magnetometer bias. This bias is learnt by the filter over time and + * persists between boots. + * + * @min -0.6 + * @max 0.6 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_Z, 0.0f); + /** * Threshold for filter initialization. * diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 5d56dbaae361..d60130786c60 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -43,12 +43,13 @@ #include #include #include -#include #ifndef M_PI_F #define M_PI_F static_cast(M_PI) #endif +#define MIN_AIRSPEED_MEAS 5.0f + constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : @@ -93,6 +94,7 @@ AttPosEKF::AttPosEKF() : correctedDelVel(), summedDelAng(), summedDelVel(), + prevDelAng(), accNavMag(), earthRateNED(), angRate(), @@ -270,12 +272,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED() delAngTotal.y += correctedDelAng.y; delAngTotal.z += correctedDelAng.z; - // Save current measurements - Vector3f prevDelAng = correctedDelAng; - // Apply corrections for earths rotation rate and coning errors // * and + operators have been overloaded - correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + prevDelAng = correctedDelAng; // Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); @@ -1688,7 +1688,7 @@ void AttPosEKF::FuseAirspeed() // Calculate the predicted airspeed VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS)) { // Calculate observation jacobians SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); @@ -1852,7 +1852,7 @@ void AttPosEKF::FuseOptFlow() Vector3f relVelSensor; // Perform sequential fusion of optical flow measurements only with valid tilt and height - flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng); float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9]; bool validTilt = Tnb.z.z > 0.71f; if (validTilt) @@ -2112,7 +2112,7 @@ void AttPosEKF::OpticalFlowEKF() } else { return; } - distanceTravelledSq = std::min(distanceTravelledSq, 100.0f); + distanceTravelledSq = fmin(distanceTravelledSq, 100.0f); Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma)); } @@ -2152,7 +2152,7 @@ void AttPosEKF::OpticalFlowEKF() varInnovRng = 1.0f/SK_RNG[1]; // constrain terrain height to be below the vehicle - flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng); // estimate range to centre of image range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2]; @@ -2172,7 +2172,7 @@ void AttPosEKF::OpticalFlowEKF() } // constrain the states flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng); // correct the covariance matrix float nextPopt[2][2]; @@ -2181,8 +2181,8 @@ void AttPosEKF::OpticalFlowEKF() nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); // prevent the state variances from becoming negative and maintain symmetry - Popt[0][0] = std::max(nextPopt[0][0],0.0f); - Popt[1][1] = std::max(nextPopt[1][1],0.0f); + Popt[0][0] = fmax(nextPopt[0][0],0.0f); + Popt[1][1] = fmax(nextPopt[1][1],0.0f); Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); Popt[1][0] = Popt[0][1]; } @@ -2221,7 +2221,7 @@ void AttPosEKF::OpticalFlowEKF() vel.z = statesAtFlowTime[6]; // constrain terrain height to be below the vehicle - flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng); // estimate range to centre of image range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z; @@ -2289,7 +2289,7 @@ void AttPosEKF::OpticalFlowEKF() } // constrain the states flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng); // correct the covariance matrix for (uint8_t i = 0; i < 2 ; i++) { @@ -2305,8 +2305,8 @@ void AttPosEKF::OpticalFlowEKF() } // prevent the state variances from becoming negative and maintain symmetry - Popt[0][0] = std::max(nextPopt[0][0],0.0f); - Popt[1][1] = std::max(nextPopt[1][1],0.0f); + Popt[0][0] = fmax(nextPopt[0][0],0.0f); + Popt[1][1] = fmax(nextPopt[1][1],0.0f); Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); Popt[1][0] = Popt[0][1]; } @@ -2596,7 +2596,7 @@ void AttPosEKF::CovarianceInit() P[13][13] = sq(0.2f*dtIMU); //Wind velocities - P[14][14] = 0.0f; + P[14][14] = 0.01f; P[15][15] = P[14][14]; //Earth magnetic field @@ -2827,7 +2827,7 @@ bool AttPosEKF::VelNEDDiverged() Vector3f delta = current_vel - gps_vel; float delta_len = delta.length(); - bool excessive = (delta_len > 20.0f); + bool excessive = (delta_len > 30.0f); current_ekf_state.error |= excessive; current_ekf_state.velOffsetExcessive = excessive; @@ -3287,6 +3287,7 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + prevDelAng.zero(); dAngIMU.zero(); dVelIMU.zero(); lastGyroOffset.zero(); @@ -3337,3 +3338,10 @@ void AttPosEKF::setIsFixedWing(const bool fixedWing) { _isFixedWing = fixedWing; } + +void AttPosEKF::get_covariance(float c[EKF_STATE_ESTIMATES]) +{ + for (unsigned int i = 0; i < EKF_STATE_ESTIMATES; i++) { + c[i] = P[i][i]; + } +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 9b23f4df44c0..6eb325d5317b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -139,6 +139,7 @@ class AttPosEKF { Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) + Vector3f prevDelAng; ///< previous delta angle, used for coning correction float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) @@ -379,6 +380,8 @@ class AttPosEKF { */ void ZeroVariables(); + void get_covariance(float c[28]); + protected: /** diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 284a099023dc..527420ba0bc1 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -38,7 +38,6 @@ */ #include "estimator_utilities.h" -#include // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely @@ -72,6 +71,9 @@ ekf_debug(const char *fmt, ...) void ekf_debug(const char *fmt, ...) { while(0){} } #endif +/* we don't want to pull in the standard lib just to swap two floats */ +void swap_var(float &d1, float &d2); + float Vector3f::length(void) const { return sqrt(x*x + y*y + z*z); @@ -108,9 +110,9 @@ void Mat3f::identity() { Mat3f Mat3f::transpose() const { Mat3f ret = *this; - std::swap(ret.x.y, ret.y.x); - std::swap(ret.x.z, ret.z.x); - std::swap(ret.y.z, ret.z.y); + swap_var(ret.x.y, ret.y.x); + swap_var(ret.x.z, ret.z.x); + swap_var(ret.y.z, ret.z.y); return ret; } @@ -223,3 +225,10 @@ Vector3f operator/(const Vector3f &vec, const float scalar) vecOut.z = vec.z / scalar; return vecOut; } + +void swap_var(float &d1, float &d2) +{ + float tmp = d1; + d1 = d2; + d2 = tmp; +} diff --git a/src/modules/fixedwing_backside/CMakeLists.txt b/src/modules/fixedwing_backside/CMakeLists.txt new file mode 100644 index 000000000000..c2c8ec649f25 --- /dev/null +++ b/src/modules/fixedwing_backside/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__fixedwing_backside + MAIN fixedwing_backside + SRCS + fixedwing_backside_main.cpp + fixedwing.cpp + params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index bbb39f20f0b1..39b624f24aab 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -58,8 +58,8 @@ BlockYawDamper::~BlockYawDamper() {}; void BlockYawDamper::update(float rCmd, float r, float outputScale) { - _rudder = outputScale*_r2Rdr.update(rCmd - - _rWashout.update(_rLowPass.update(r))); + _rudder = outputScale * _r2Rdr.update(rCmd - + _rWashout.update(_rLowPass.update(r))); } BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) : @@ -79,9 +79,9 @@ BlockStabilization::~BlockStabilization() {}; void BlockStabilization::update(float pCmd, float qCmd, float rCmd, float p, float q, float r, float outputScale) { - _aileron = outputScale*_p2Ail.update( + _aileron = outputScale * _p2Ail.update( pCmd - _pLowPass.update(p)); - _elevator = outputScale*_q2Elv.update( + _elevator = outputScale * _q2Elv.update( qCmd - _qLowPass.update(q)); _yawDamper.update(rCmd, r, outputScale); } @@ -127,7 +127,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par void BlockMultiModeBacksideAutopilot::update() { // wait for a sensor update, check for exit condition every 100 ms - if (poll(&_attPoll, 1, 100) < 0) return; // poll error + if (poll(&_attPoll, 1, 100) < 0) { return; } // poll error uint64_t newTimeStamp = hrt_absolute_time(); float dt = (newTimeStamp - _timeStamp) / 1.0e6f; @@ -135,31 +135,34 @@ void BlockMultiModeBacksideAutopilot::update() // check for sane values of dt // to prevent large control responses - if (dt > 1.0f || dt < 0) return; + if (dt > 1.0f || dt < 0) { return; } // set dt for all child blocks setDt(dt); // store old position command before update if new command sent if (_missionCmd.updated()) { - _lastMissionCmd = _missionCmd.getData(); + _lastMissionCmd = _missionCmd.get(); } // check for new updates - if (_param_update.updated()) updateParams(); + if (_param_update.updated()) { updateParams(); } // get new information from subscriptions updateSubscriptions(); // default all output to zero unless handled by mode - for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) - _actuators.control[i] = 0.0f; + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { + _actuators.get().control[i] = 0.0f; + } // only update guidance in auto mode - if (_status.main_state == MAIN_STATE_AUTO) { + if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here? // update guidance - _guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current); + _guide.update(_pos.get(), _att.get(), + _missionCmd.get().current, + _lastMissionCmd.current); } // XXX handle STABILIZED (loiter on spot) as well @@ -167,49 +170,51 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.main_state == MAIN_STATE_AUTO) { + if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO) { // calculate velocity, XXX should be airspeed, - // but using ground speed for now for the purpose + // but using ground speed for now for the purpose // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vel_n * _pos.vel_n + - _pos.vel_e * _pos.vel_e + - _pos.vel_d * _pos.vel_d)); + _pos.get().vel_n * _pos.get().vel_n + + _pos.get().vel_e * _pos.get().vel_e + + _pos.get().vel_d * _pos.get().vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); // altitude hold - float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt); + float dThrottle = _h2Thr.update(_missionCmd.get().current.alt - _pos.get().alt); // heading hold - float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); + float psiError = _wrap_pi(_guide.getPsiCmd() - _att.get().yaw); float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); - float pCmd = _phi2P.update(phiCmd - _att.roll); + float pCmd = _phi2P.update(phiCmd - _att.get().roll); // velocity hold // negative sign because nose over to increase speed float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.pitch); + float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch); // yaw rate cmd float rCmd = 0; // stabilization - float velocityRatio = _trimV.get()/v; - float outputScale = velocityRatio*velocityRatio; + float velocityRatio = _trimV.get() / v; + float outputScale = velocityRatio * velocityRatio; // this term scales the output based on the dynamic pressure change from trim _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed, + _att.get().rollspeed, + _att.get().pitchspeed, + _att.get().yawspeed, outputScale); // output - _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); - _actuators.control[CH_THR] = dThrottle + _trimThr.get(); + _actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + _actuators.get().control[CH_THR] = dThrottle + _trimThr.get(); // XXX limit throttle to manual setting (safety) for now. // If it turns out to be confusing, it can be removed later once @@ -217,28 +222,29 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. // do not limit in HIL - if (_status.hil_state != HIL_STATE_ON) { + if (_status.get().hil_state != _vehicle_status::HIL_STATE_ON) { /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; + _actuators.get().control[CH_THR] = + (_actuators.get().control[CH_THR] < _manual.get().z) ? + _actuators.get().control[CH_THR] : _manual.get().z; } - } else if (_status.main_state == MAIN_STATE_MANUAL) { - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; + } else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_MANUAL) { + _actuators.get().control[CH_AIL] = _manual.get().y; + _actuators.get().control[CH_ELV] = _manual.get().x; + _actuators.get().control[CH_RDR] = _manual.get().r; + _actuators.get().control[CH_THR] = _manual.get().z; - } else if (_status.main_state == MAIN_STATE_ALTCTL || - _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { + } else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_ALTCTL || + _status.get().main_state == vehicle_status_s::MAIN_STATE_POSCTL /* TODO, implement pos control */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vel_n * _pos.vel_n + - _pos.vel_e * _pos.vel_e + - _pos.vel_d * _pos.vel_d)); + _pos.get().vel_n * _pos.get().vel_n + + _pos.get().vel_e * _pos.get().vel_e + + _pos.get().vel_d * _pos.get().vel_d)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding @@ -247,33 +253,35 @@ void BlockMultiModeBacksideAutopilot::update() //_crMax.get()*_manual.pitch - _pos.vz); // roll channel -> bank angle - float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); - float pCmd = _phi2P.update(phiCmd - _att.roll); + float phiCmd = _phiLimit.update(_manual.get().y * _phiLimit.getMax()); + float pCmd = _phi2P.update(phiCmd - _att.get().roll); // throttle channel -> velocity // negative sign because nose over to increase speed - float vCmd = _vLimit.update(_manual.throttle * - (_vLimit.getMax() - _vLimit.getMin()) + - _vLimit.getMin()); + float vCmd = _vLimit.update(_manual.get().z * + (_vLimit.getMax() - _vLimit.getMin()) + + _vLimit.getMin()); float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.pitch); + float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch); // yaw rate cmd float rCmd = 0; // stabilization _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _att.get().rollspeed, + _att.get().pitchspeed, + _att.get().yawspeed); // output - _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + _actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); // currently using manual throttle // XXX if you enable this watch out, vz might be very noisy //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); - _actuators.control[CH_THR] = _manual.throttle; + _actuators.get().control[CH_THR] = _manual.get().z; // XXX limit throttle to manual setting (safety) for now. // If it turns out to be confusing, it can be removed later once @@ -281,22 +289,26 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (_status.hil_state != HIL_STATE_ON) { + if (_status.get().hil_state != vehicle_status_s::HIL_STATE_ON) { /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; + _actuators.get().control[CH_THR] = + (_actuators.get().control[CH_THR] < _manual.get().z) ? + _actuators.get().control[CH_THR] : _manual.get().z; } - // body rates controller, disabled for now - // TODO - } else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here? - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + // body rates controller, disabled for now + // TODO + + } else if ( + 0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here? - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; + _stabilization.update(_manual.get().y, _manual.get().x, _manual.get().r, + _att.get().rollspeed, _att.get().pitchspeed, _att.get().yawspeed); + + _actuators.get().control[CH_AIL] = _stabilization.getAileron(); + _actuators.get().control[CH_ELV] = _stabilization.getElevator(); + _actuators.get().control[CH_RDR] = _stabilization.getRudder(); + _actuators.get().control[CH_THR] = _manual.get().z; } // update all publications @@ -307,8 +319,9 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() { // send one last publication when destroyed, setting // all output to zero - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - _actuators.control[i] = 0.0f; + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { + _actuators.get().control[i] = 0.0f; + } updatePublications(); } diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 06559de9790b..fe9a1066bd4f 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -79,8 +79,9 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: fixedwing_backside {start|stop|status} [-p ]\n\n"); exit(1); @@ -112,11 +113,11 @@ int fixedwing_backside_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd("fixedwing_backside", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 5120, - control_demo_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 5120, + control_demo_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt new file mode 100644 index 000000000000..b093c71ceed2 --- /dev/null +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__fw_att_control + MAIN fw_att_control + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + fw_att_control_main.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 4159ebaf2233..8772ceb835ea 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,6 +42,9 @@ */ #include +#include +#include +#include #include #include #include @@ -55,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -64,7 +66,7 @@ #include #include #include -#include +#include #include #include #include @@ -80,6 +82,7 @@ #include #include #include +#include #include /** @@ -122,11 +125,10 @@ class FixedwingAttitudeControl bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle */ - int _att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ int _vcontrol_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ @@ -141,12 +143,11 @@ class FixedwingAttitudeControl orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure - struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ @@ -160,18 +161,19 @@ class FixedwingAttitudeControl bool _setpoint_valid; /**< flag if the position control setpoint is valid */ bool _debug; /**< if set to true, print debug output */ + float _flaps_cmd_last; + float _flaperons_cmd_last; + + struct { float tconst; float p_p; - float p_d; float p_i; float p_ff; float p_rmax_pos; float p_rmax_neg; float p_integrator_max; - float p_roll_feedforward; float r_p; - float r_d; float r_i; float r_ff; float r_integrator_max; @@ -180,11 +182,15 @@ class FixedwingAttitudeControl float y_i; float y_d; float y_ff; - float y_roll_feedforward; float y_integrator_max; float y_coordinated_min_speed; int32_t y_coordinated_method; float y_rmax; + float w_p; + float w_i; + float w_ff; + float w_integrator_max; + float w_rmax; float airspeed_min; float airspeed_trim; @@ -193,12 +199,17 @@ class FixedwingAttitudeControl float trim_roll; float trim_pitch; float trim_yaw; - float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ - float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ - float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ - float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float man_roll_max; /**< Max Roll in rad */ - float man_pitch_max; /**< Max Pitch in rad */ + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ + + float flaps_scale; /**< Scale factor for flaps */ + float flaperon_scale; /**< Scale factor for flaperons */ + + int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ } _parameters; /**< local copies of interesting parameters */ @@ -206,15 +217,12 @@ class FixedwingAttitudeControl param_t tconst; param_t p_p; - param_t p_d; param_t p_i; param_t p_ff; param_t p_rmax_pos; param_t p_rmax_neg; param_t p_integrator_max; - param_t p_roll_feedforward; param_t r_p; - param_t r_d; param_t r_i; param_t r_ff; param_t r_integrator_max; @@ -223,11 +231,15 @@ class FixedwingAttitudeControl param_t y_i; param_t y_d; param_t y_ff; - param_t y_roll_feedforward; param_t y_integrator_max; param_t y_coordinated_min_speed; param_t y_coordinated_method; param_t y_rmax; + param_t w_p; + param_t w_i; + param_t w_ff; + param_t w_integrator_max; + param_t w_rmax; param_t airspeed_min; param_t airspeed_trim; @@ -241,12 +253,23 @@ class FixedwingAttitudeControl param_t man_roll_max; param_t man_pitch_max; + param_t flaps_scale; + param_t flaperon_scale; + + param_t vtol_type; + } _parameter_handles; /**< handles for interesting parameters */ + // Rotation matrix and euler angles to extract from control state + math::Matrix<3, 3> _R; + float _roll; + float _pitch; + float _yaw; ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; + ECL_WheelController _wheel_ctrl; /** @@ -270,12 +293,6 @@ class FixedwingAttitudeControl */ void vehicle_manual_poll(); - - /** - * Check for airspeed updates. - */ - void vehicle_airspeed_poll(); - /** * Check for accel updates. */ @@ -327,9 +344,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _control_task(-1), /* subscriptions */ - _att_sub(-1), + _ctrl_state_sub(-1), _accel_sub(-1), - _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), _manual_sub(-1), @@ -351,15 +367,16 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ _setpoint_valid(false), - _debug(false) + _debug(false), + _flaps_cmd_last(0), + _flaperons_cmd_last(0) { /* safely initialize structs */ - _att = {}; + _ctrl_state = {}; _accel = {}; _att_sp = {}; _rates_sp = {}; _manual = {}; - _airspeed = {}; _vcontrol_mode = {}; _actuators = {}; _actuators_airframe = {}; @@ -374,7 +391,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX"); - _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); _parameter_handles.r_p = param_find("FW_RR_P"); _parameter_handles.r_i = param_find("FW_RR_I"); @@ -388,6 +404,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); + _parameter_handles.w_p = param_find("FW_WR_P"); + _parameter_handles.w_i = param_find("FW_WR_I"); + _parameter_handles.w_ff = param_find("FW_WR_FF"); + _parameter_handles.w_integrator_max = param_find("FW_WR_IMAX"); + _parameter_handles.w_rmax = param_find("FW_W_RMAX"); + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); @@ -404,6 +426,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); + _parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL"); + + _parameter_handles.vtol_type = param_find("VT_TYPE"); + /* fetch initial parameter values */ parameters_update(); } @@ -424,7 +451,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -448,7 +475,6 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); - param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); @@ -465,6 +491,12 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); + param_get(_parameter_handles.w_p, &(_parameters.w_p)); + param_get(_parameter_handles.w_i, &(_parameters.w_i)); + param_get(_parameter_handles.w_ff, &(_parameters.w_ff)); + param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max)); + param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax)); + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); @@ -481,6 +513,11 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale); + param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale); + + param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -489,7 +526,6 @@ FixedwingAttitudeControl::parameters_update() _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); - _pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward); /* roll control parameters */ _roll_ctrl.set_time_constant(_parameters.tconst); @@ -508,6 +544,13 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + /* wheel control parameters */ + _wheel_ctrl.set_k_p(_parameters.w_p); + _wheel_ctrl.set_k_i(_parameters.w_i); + _wheel_ctrl.set_k_ff(_parameters.w_ff); + _wheel_ctrl.set_integrator_max(_parameters.w_integrator_max); + _wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax)); + return OK; } @@ -539,18 +582,6 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -void -FixedwingAttitudeControl::vehicle_airspeed_poll() -{ - /* check if there is a new position */ - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - } -} - void FixedwingAttitudeControl::vehicle_accel_poll() { @@ -623,9 +654,8 @@ FixedwingAttitudeControl::task_main() * do subscriptions */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -634,40 +664,40 @@ FixedwingAttitudeControl::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - /* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */ - orb_set_interval(_att_sub, 17); + /* do not limit the attitude updates in order to minimize latency. + * actuator outputs are still limited by the individual drivers + * properly to not saturate IO or physical limitations */ parameters_update(); /* get an initial update for all sensor and status data */ - vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); vehicle_status_poll(); - /* wakeup source(s) */ - struct pollfd fds[2]; + /* wakeup source */ + px4_pollfd_struct_t fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; - fds[1].fd = _att_sub; + fds[1].fd = _ctrl_state_sub; fds[1].events = POLLIN; _task_running = true; while (!_task_should_exit) { - static int loop_counter = 0; /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -689,8 +719,6 @@ FixedwingAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -700,12 +728,21 @@ FixedwingAttitudeControl::task_main() deltaT = 0.01f; /* load local copies */ - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); - if (_vehicle_status.is_vtol) { - /* vehicle type is VTOL, need to modify attitude! - * The following modification to the attitude is vehicle specific and in this case applies - * to tail-sitter models !!! + + /* get current rotation matrix and euler angles from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R = q_att.to_dcm(); + + math::Vector<3> euler_angles; + euler_angles = _R.to_euler(); + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); + + if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { + /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * * Since the VTOL airframe is initialized as a multicopter we need to * modify the estimated attitude for the fixed wing operation. @@ -721,50 +758,36 @@ FixedwingAttitudeControl::task_main() * Rxy Ryy Rzy -Rzy Ryy Rxy * Rxz Ryz Rzz -Rzz Ryz Rxz * */ - math::Matrix<3, 3> R; //original rotation matrix - math::Matrix<3, 3> R_adapted; //modified rotation matrix - R.set(_att.R); - R_adapted.set(_att.R); + math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix /* move z to x */ - R_adapted(0, 0) = R(0, 2); - R_adapted(1, 0) = R(1, 2); - R_adapted(2, 0) = R(2, 2); + R_adapted(0, 0) = _R(0, 2); + R_adapted(1, 0) = _R(1, 2); + R_adapted(2, 0) = _R(2, 2); /* move x to z */ - R_adapted(0, 2) = R(0, 0); - R_adapted(1, 2) = R(1, 0); - R_adapted(2, 2) = R(2, 0); + R_adapted(0, 2) = _R(0, 0); + R_adapted(1, 2) = _R(1, 0); + R_adapted(2, 2) = _R(2, 0); /* change direction of pitch (convert to right handed system) */ R_adapted(0, 0) = -R_adapted(0, 0); R_adapted(1, 0) = -R_adapted(1, 0); R_adapted(2, 0) = -R_adapted(2, 0); - math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation - euler_angles = R_adapted.to_euler(); + euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation /* fill in new attitude data */ - _att.roll = euler_angles(0); - _att.pitch = euler_angles(1); - _att.yaw = euler_angles(2); - PX4_R(_att.R, 0, 0) = R_adapted(0, 0); - PX4_R(_att.R, 0, 1) = R_adapted(0, 1); - PX4_R(_att.R, 0, 2) = R_adapted(0, 2); - PX4_R(_att.R, 1, 0) = R_adapted(1, 0); - PX4_R(_att.R, 1, 1) = R_adapted(1, 1); - PX4_R(_att.R, 1, 2) = R_adapted(1, 2); - PX4_R(_att.R, 2, 0) = R_adapted(2, 0); - PX4_R(_att.R, 2, 1) = R_adapted(2, 1); - PX4_R(_att.R, 2, 2) = R_adapted(2, 2); + _R = R_adapted; + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); /* lastly, roll- and yawspeed have to be swaped */ - float helper = _att.rollspeed; - _att.rollspeed = -_att.yawspeed; - _att.yawspeed = helper; + float helper = _ctrl_state.roll_rate; + _ctrl_state.roll_rate = -_ctrl_state.yaw_rate; + _ctrl_state.yaw_rate = helper; } - vehicle_airspeed_poll(); - vehicle_setpoint_poll(); vehicle_accel_poll(); @@ -777,6 +800,10 @@ FixedwingAttitudeControl::task_main() vehicle_status_poll(); + // the position controller will not emit attitude setpoints in some modes + // we need to make sure that this flag is reset + _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; + /* lock integrator until control is started */ bool lock_integrator; @@ -796,24 +823,77 @@ FixedwingAttitudeControl::task_main() //warnx("_actuators_airframe.control[1] = -1.0f;"); } - /* decide if in stabilized or full manual control */ + /* if we are in rotary wing mode, do nothing */ + if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) { + continue; + } - if (_vcontrol_mode.flag_control_attitude_enabled) { + /* default flaps to center */ + float flaps_control = 0.0f; - /* scale around tuning airspeed */ + static float delta_flaps = 0; + + /* map flaps by default to manual if valid */ + if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { + flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; + } + + // move the actual control value continuous with time + static hrt_abstime t_flaps_changed = 0; + if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) { + t_flaps_changed = hrt_absolute_time(); + delta_flaps = flaps_control - _flaps_cmd_last; + _flaps_cmd_last = flaps_control; + } + + static float flaps_applied = 0.0f; + + if (fabsf(flaps_applied - flaps_control) > 0.01f) { + flaps_applied = (flaps_control - delta_flaps) + (float)hrt_elapsed_time(&t_flaps_changed) * (delta_flaps) / 1000000; + } + /* default flaperon to center */ + float flaperon = 0.0f; + + static float delta_flaperon = 0.0f; + + /* map flaperons by default to manual if valid */ + if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { + flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; + } + + // move the actual control value continuous with time + static hrt_abstime t_flaperons_changed = 0; + if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) { + t_flaperons_changed = hrt_absolute_time(); + delta_flaperon = flaperon - _flaperons_cmd_last; + _flaperons_cmd_last = flaperon; + } + + static float flaperon_applied = 0.0f; + + if (fabsf(flaperon_applied - flaperon) > 0.01f) { + flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * (delta_flaperon) / 1000000; + } + + /* decide if in stabilized or full manual control */ + if (_vcontrol_mode.flag_control_attitude_enabled) { + /* scale around tuning airspeed */ float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) { airspeed = _parameters.airspeed_trim; if (nonfinite) { perf_count(_nonfinite_input_perf); } } else { /* prevent numerical drama by requiring 0.5 m/s minimal speed */ - airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); + airspeed = math::max(0.5f, _ctrl_state.airspeed); } /* @@ -823,11 +903,19 @@ FixedwingAttitudeControl::task_main() * * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); + /* Use min airspeed to calculate ground speed scaling region. + * Don't scale below gspd_scaling_trim + */ + float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + + _global_pos.vel_e * _global_pos.vel_e); + float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); + float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); + float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; + float yaw_sp = 0.0f; float yaw_manual = 0.0f; float throttle_sp = 0.0f; @@ -841,6 +929,7 @@ FixedwingAttitudeControl::task_main() /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + yaw_sp = _att_sp.yaw_body; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -852,13 +941,40 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { + + /* the pilot does not want to change direction, + * take straight attitude setpoint from position controller + */ + if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) { + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + } else { + roll_sp = (_manual.y * _parameters.man_roll_max) + + _parameters.rollsp_offset_rad; + } + + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); + } + + } else if (_vcontrol_mode.flag_control_altitude_enabled) { /* * Velocity should be controlled and manual is enabled. */ - roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; + roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; @@ -871,6 +987,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else { /* @@ -885,14 +1002,11 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; - pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) - + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad; /* allow manual control of rudder deflection */ yaw_manual = _manual.r; throttle_sp = _manual.z; - _actuators.control[4] = _manual.flaps; /* * in manual mode no external source should / does emit attitude setpoints. @@ -907,13 +1021,15 @@ FixedwingAttitudeControl::task_main() att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); - - } else if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing) { - /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + if (_attitude_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); + + } else { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } } } @@ -922,30 +1038,22 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } /* Prepare speed_body_u and speed_body_w */ - float speed_body_u = 0.0f; - float speed_body_v = 0.0f; - float speed_body_w = 0.0f; - if(_att.R_valid) { - speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d; - speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d; - speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d; - } else { - if (_debug && loop_counter % 10 == 0) { - warnx("Did not get a valid R\n"); - } - } + float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d; + float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d; + float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d; /* Prepare data for attitude controllers */ struct ECL_ControlData control_input = {}; - control_input.roll = _att.roll; - control_input.pitch = _att.pitch; - control_input.yaw = _att.yaw; - control_input.roll_rate = _att.rollspeed; - control_input.pitch_rate = _att.pitchspeed; - control_input.yaw_rate = _att.yawspeed; + control_input.roll = _roll; + control_input.pitch = _pitch; + control_input.yaw = _yaw; + control_input.roll_rate = _ctrl_state.roll_rate; + control_input.pitch_rate = _ctrl_state.pitch_rate; + control_input.yaw_rate = _ctrl_state.yaw_rate; control_input.speed_body_u = speed_body_u; control_input.speed_body_v = speed_body_v; control_input.speed_body_w = speed_body_w; @@ -954,17 +1062,23 @@ FixedwingAttitudeControl::task_main() control_input.acc_body_z = _accel.z; control_input.roll_setpoint = roll_sp; control_input.pitch_setpoint = pitch_sp; + control_input.yaw_setpoint = yaw_sp; control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; + control_input.groundspeed = groundspeed; + control_input.groundspeed_scaler = groundspeed_scaler; + + _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); /* Run attitude controllers */ - if (isfinite(roll_sp) && isfinite(pitch_sp)) { + if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude + _wheel_ctrl.control_attitude(control_input); /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); @@ -973,8 +1087,8 @@ FixedwingAttitudeControl::task_main() /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(control_input); - _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; - if (!isfinite(roll_u)) { + _actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!PX4_ISFINITE(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); @@ -984,8 +1098,8 @@ FixedwingAttitudeControl::task_main() } float pitch_u = _pitch_ctrl.control_bodyrate(control_input); - _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; - if (!isfinite(pitch_u)) { + _actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; + if (!PX4_ISFINITE(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { @@ -1004,13 +1118,21 @@ FixedwingAttitudeControl::task_main() } } - float yaw_u = _yaw_ctrl.control_bodyrate(control_input); - _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + float yaw_u = 0.0f; + if (_att_sp.fw_control_yaw == true) { + yaw_u = _wheel_ctrl.control_bodyrate(control_input); + } + + else { + yaw_u = _yaw_ctrl.control_bodyrate(control_input); + } + _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[2] += yaw_manual; - if (!isfinite(yaw_u)) { + if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); @@ -1019,11 +1141,11 @@ FixedwingAttitudeControl::task_main() /* throttle passed through if it is finite and if no engine failure was * detected */ - _actuators.control[3] = (isfinite(throttle_sp) && + _actuators.control[3] = (PX4_ISFINITE(throttle_sp) && !(_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd)) ? throttle_sp : 0.0f; - if (!isfinite(throttle_sp)) { + if (!PX4_ISFINITE(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } @@ -1055,27 +1177,27 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ - _actuators.control[0] = _manual.y; - _actuators.control[1] = -_manual.x; - _actuators.control[2] = _manual.r; - _actuators.control[3] = _manual.z; - _actuators.control[4] = _manual.flaps; + _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll; + _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch; + _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } + _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_applied; _actuators.control[5] = _manual.aux1; - _actuators.control[6] = _manual.aux2; + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon_applied; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _att.timestamp; + _actuators.timestamp_sample = _ctrl_state.timestamp; _actuators_airframe.timestamp = hrt_absolute_time(); - _actuators_airframe.timestamp_sample = _att.timestamp; + _actuators_airframe.timestamp_sample = _ctrl_state.timestamp; /* Only publish if any of the proper modes are enabled */ if(_vcontrol_mode.flag_control_rates_enabled || - _vcontrol_mode.flag_control_attitude_enabled || - _vcontrol_mode.flag_control_manual_enabled) + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub != nullptr) { @@ -1103,7 +1225,6 @@ FixedwingAttitudeControl::task_main() _control_task = -1; _task_running = false; - _exit(0); } int @@ -1115,8 +1236,8 @@ FixedwingAttitudeControl::start() _control_task = px4_task_spawn_cmd("fw_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 1600, - (main_t)&FixedwingAttitudeControl::task_main_trampoline, + 1300, + (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -1130,23 +1251,29 @@ FixedwingAttitudeControl::start() int fw_att_control_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: fw_att_control {start|stop|status}"); + warnx("usage: fw_att_control {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { - if (att_control::g_control != nullptr) - errx(1, "already running"); + if (att_control::g_control != nullptr) { + warnx("already running"); + return 1; + } att_control::g_control = new FixedwingAttitudeControl; - if (att_control::g_control == nullptr) - errx(1, "alloc failed"); + if (att_control::g_control == nullptr) { + warnx("alloc failed"); + return 1; + } if (OK != att_control::g_control->start()) { delete att_control::g_control; att_control::g_control = nullptr; - err(1, "start failed"); + warn("start failed"); + return 1; } /* check if the waiting is necessary at all */ @@ -1160,24 +1287,28 @@ int fw_att_control_main(int argc, char *argv[]) } printf("\n"); } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { - if (att_control::g_control == nullptr) - errx(1, "not running"); + if (att_control::g_control == nullptr){ + warnx("not running"); + return 1; + } delete att_control::g_control; att_control::g_control = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (att_control::g_control) { - errx(0, "running"); + warnx("running"); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index dbf40230bab7..ce2db9ad5c7c 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -40,11 +40,6 @@ * @author Thomas Gubler */ -#include - -#include - - /* * Controller parameters, accessible via MAVLink * @@ -64,7 +59,7 @@ * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); +PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.4f); /** * Pitch rate proportional gain. @@ -76,7 +71,7 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); /** * Pitch rate integrator gain. @@ -84,11 +79,11 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); * This gain defines how much control response will result out of a steady * state error. It trims any constant error. * - * @min 0.0 - * @max 50.0 + * @min 0.005 + * @max 0.5 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_I, 0.02f); /** * Maximum positive / up pitch rate. @@ -101,7 +96,7 @@ PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f); * @max 90.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f); /** * Maximum negative / down pitch rate. @@ -114,7 +109,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); * @max 90.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f); /** * Pitch rate integrator limit @@ -126,18 +121,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); - -/** - * Roll to Pitch feedforward gain. - * - * This compensates during turns and ensures the nose stays level. - * - * @min 0.0 - * @max 2.0 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); /** * Roll rate proportional Gain @@ -157,11 +141,11 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); * This gain defines how much control response will result out of a steady * state error. It trims any constant error. * - * @min 0.0 - * @max 100.0 + * @min 0.005 + * @max 0.2 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_RR_I, 0.01f); /** * Roll Integrator Anti-Windup @@ -185,7 +169,7 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); * @max 90.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f); +PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f); /** * Yaw rate proportional gain @@ -236,6 +220,55 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); */ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); +/** + * Wheel steering rate proportional gain + * + * This defines how much the wheel steering input will be commanded depending on the + * current body angular rate error. + * + * @min 0.005 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f); + +/** + * Wheel steering rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 50.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); + +/** + * Wheel steering rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f); + +/** + * Maximum wheel steering rate + * + * This limits the maximum wheel steering rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f); + /** * Roll rate feed forward * @@ -247,7 +280,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); * @max 10.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f); +PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); /** * Pitch rate feed forward @@ -258,7 +291,7 @@ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f); * @max 10.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f); +PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); /** * Yaw rate feed forward @@ -271,6 +304,17 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f); */ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); +/** + * Wheel steering rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); + /** * Minimal speed for yaw coordination * @@ -390,3 +434,21 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); + +/** + * Scale factor for flaps + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f); + +/** + * Scale factor for flaperons + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f); diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt new file mode 100644 index 000000000000..e74b6e4b37ca --- /dev/null +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__fw_pos_control_l1 + MAIN fw_pos_control_l1 + STACK 1200 + COMPILE_FLAGS + -Wno-float-equal + -Os + SRCS + fw_pos_control_l1_main.cpp + landingslope.cpp + mtecs/mTecs.cpp + mtecs/limitoverride.cpp + DEPENDS + platforms__common + lib__external_lgpl + lib__ecl + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 9453b51aedeb..24cf1f084fa1 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -49,9 +49,13 @@ * * @author Lorenz Meier * @author Thomas Gubler + * @author Andreas Antener */ #include +#include +#include +#include #include #include #include @@ -65,19 +69,19 @@ #include #include #include -#include #include #include #include #include #include #include -#include +#include #include #include #include #include #include +#include #include #include #include @@ -92,9 +96,20 @@ #include "landingslope.h" #include "mtecs/mTecs.h" #include +#include static int _control_task = -1; /**< task handle for sensor task */ - +#define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode +#define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode +#define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane +#define HDG_HOLD_YAWRATE_THRESH 0.15f // max yawrate at which plane locks yaw for heading hold mode +#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading +#define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) +#define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid + +static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes +static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode +static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; /** * L1 control app start / stop handling function @@ -140,8 +155,7 @@ class FixedwingPositionControl int _global_pos_sub; int _pos_sp_triplet_sub; - int _att_sub; /**< vehicle attitude subscription */ - int _airspeed_sub; /**< airspeed subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _control_mode_sub; /**< control mode subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -152,11 +166,10 @@ class FixedwingPositionControl orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< control mode */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ @@ -165,7 +178,15 @@ class FixedwingPositionControl perf_counter_t _loop_perf; /**< loop performance counter */ - float _hold_alt; /**< hold altitude for velocity mode */ + float _hold_alt; /**< hold altitude for altitude mode */ + float _takeoff_ground_alt; /**< ground altitude at which plane was launched */ + float _hdg_hold_yaw; /**< hold heading for velocity mode */ + bool _hdg_hold_enabled; /**< heading hold enabled */ + bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ + float _althold_epv; /**< the position estimate accuracy when engaging alt hold */ + bool _was_in_deadband; /**< wether the last stick input was in althold deadband */ + struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ + struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** _R_nb; ///< current attitude + float _roll; + float _pitch; + float _yaw; ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -205,6 +240,7 @@ class FixedwingPositionControl enum FW_POSCTRL_MODE { FW_POSCTRL_MODE_AUTO, FW_POSCTRL_MODE_POSITION, + FW_POSCTRL_MODE_ALTITUDE, FW_POSCTRL_MODE_OTHER } _control_mode_current; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. @@ -252,6 +288,7 @@ class FixedwingPositionControl float land_flare_pitch_min_deg; float land_flare_pitch_max_deg; int land_use_terrain_estimate; + float land_airspeed_scale; } _parameters; /**< local copies of interesting parameters */ @@ -300,6 +337,7 @@ class FixedwingPositionControl param_t land_flare_pitch_min_deg; param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; + param_t land_airspeed_scale; } _parameter_handles; /**< handles for interesting parameters */ @@ -331,14 +369,9 @@ class FixedwingPositionControl bool vehicle_manual_control_setpoint_poll(); /** - * Check for airspeed updates. + * Check for changes in control state. */ - bool vehicle_airspeed_poll(); - - /** - * Check for position updates. - */ - void vehicle_attitude_poll(); + void control_state_poll(); /** * Check for accel updates. @@ -353,12 +386,48 @@ class FixedwingPositionControl /** * Publish navigation capabilities */ - void navigation_capabilities_publish(); + void navigation_capabilities_publish(); + + /** + * Get a new waypoint based on heading and distance from current position + * + * @param heading the heading to fly to + * @param distance the distance of the generated waypoint + * @param waypoint_prev the waypoint at the current position + * @param waypoint_next the waypoint in the heading direction + */ + void get_waypoint_heading_distance(float heading, float distance, + struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init); /** * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available */ - float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); + float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); + + /** + * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available + */ + float get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos); + + /** + * Check if we are in a takeoff situation + */ + bool in_takeoff_situation(); + + /** + * Do takeoff help when in altitude controlled modes + * @param hold_altitude altitude setpoint for controller + * @param pitch_limit_min minimum pitch allowed + */ + void do_takeoff_help(float *hold_altitude, float *pitch_limit_min); + + /** + * Update desired altitude base on user pitch stick input + * + * @param dt Time step + * @return true if climbout mode was requested by user (climb with max rate and min airspeed) + */ + bool update_desired_altitude(float dt); /** * Control position. @@ -366,8 +435,8 @@ class FixedwingPositionControl bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); - float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); + float calculate_target_airspeed(float airspeed_demand); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -382,12 +451,12 @@ class FixedwingPositionControl /* * Reset takeoff state */ - void reset_takeoff_state(); + void reset_takeoff_state(); /* * Reset landing state */ - void reset_landing_state(); + void reset_landing_state(); /* * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter) @@ -425,8 +494,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), _pos_sp_triplet_sub(-1), - _att_sub(-1), - _airspeed_sub(-1), + _ctrl_state_sub(-1), _control_mode_sub(-1), _vehicle_status_sub(-1), _params_sub(-1), @@ -439,11 +507,10 @@ FixedwingPositionControl::FixedwingPositionControl() : _nav_capabilities_pub(nullptr), /* states */ - _att(), + _ctrl_state(), _att_sp(), _nav_capabilities(), _manual(), - _airspeed(), _control_mode(), _vehicle_status(), _global_pos(), @@ -454,6 +521,14 @@ FixedwingPositionControl::FixedwingPositionControl() : _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), _hold_alt(0.0f), + _takeoff_ground_alt(0.0f), + _hdg_hold_yaw(0.0f), + _hdg_hold_enabled(false), + _yaw_lock_engaged(false), + _althold_epv(0.0f), + _was_in_deadband(false), + _hdg_hold_prev_wp{}, + _hdg_hold_curr_wp{}, _control_position_last_called(0), land_noreturn_horizontal(false), @@ -462,6 +537,13 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), land_useterrain(false), + _t_alt_prev_valid(0), + _time_last_t_alt(0), + _time_started_landing(0), + height_flare(0.0f), + _was_in_air(false), + _time_went_in_air(0), + _runway_takeoff(), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), @@ -503,6 +585,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); + _parameter_handles.land_airspeed_scale = param_find("FW_AIRSPD_SCALE"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -542,7 +625,7 @@ FixedwingPositionControl::~FixedwingPositionControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -606,6 +689,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); + param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -656,6 +740,8 @@ FixedwingPositionControl::parameters_update() /* Update the mTecs */ _mTecs.updateParams(); + _runway_takeoff.updateParams(); + return OK; } @@ -683,32 +769,6 @@ FixedwingPositionControl::vehicle_status_poll() } } -bool -FixedwingPositionControl::vehicle_airspeed_poll() -{ - /* check if there is an airspeed update or if it timed out */ - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - _airspeed_valid = true; - _airspeed_last_valid = hrt_absolute_time(); - - } else { - - /* no airspeed updates for one second */ - if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { - _airspeed_valid = false; - } - } - - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); - - return airspeed_updated; -} - bool FixedwingPositionControl::vehicle_manual_control_setpoint_poll() { @@ -724,21 +784,38 @@ FixedwingPositionControl::vehicle_manual_control_setpoint_poll() return manual_updated; } - void -FixedwingPositionControl::vehicle_attitude_poll() +FixedwingPositionControl::control_state_poll() { /* check if there is a new position */ - bool att_updated; - orb_check(_att_sub, &att_updated); + bool ctrl_state_updated; + orb_check(_ctrl_state_sub, &ctrl_state_updated); - if (att_updated) { - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + if (ctrl_state_updated) { + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + _airspeed_valid = true; + _airspeed_last_valid = hrt_absolute_time(); + + } else { - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _R_nb(i, j) = PX4_R(_att.R, i, j); + /* no airspeed updates for one second */ + if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { + _airspeed_valid = false; + } } + + /* set rotation matrix and euler angles */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R_nb = q_att.to_dcm(); + + math::Vector<3> euler_angles; + euler_angles = _R_nb.to_euler(); + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); + + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); } void @@ -787,7 +864,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) float airspeed; if (_airspeed_valid) { - airspeed = _airspeed.true_airspeed_m_s; + airspeed = _ctrl_state.airspeed; } else { airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; @@ -855,6 +932,8 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c void FixedwingPositionControl::navigation_capabilities_publish() { + _nav_capabilities.timestamp = hrt_absolute_time(); + if (_nav_capabilities_pub != nullptr) { orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); } else { @@ -862,17 +941,42 @@ void FixedwingPositionControl::navigation_capabilities_publish() } } +void FixedwingPositionControl::get_waypoint_heading_distance(float heading, float distance, + struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init) +{ + waypoint_prev.valid = true; + waypoint_prev.alt = _hold_alt; + + if (flag_init) { + // on init set first waypoint to momentary position + waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; + } else { + /* + for previous waypoint use the one still in front of us but shift it such that it is + located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us + */ + waypoint_prev.lat = waypoint_next.lat - cos(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_prev.lon = waypoint_next.lon - sin(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6; + } + + waypoint_next.valid = true; + waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_next.alt = _hold_alt; +} + float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { - if (!isfinite(global_pos.terrain_alt)) { + if (!PX4_ISFINITE(global_pos.terrain_alt)) { return land_setpoint_alt; } /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ - if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { + if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) { if(!land_useterrain) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); + mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; } return global_pos.terrain_alt; @@ -881,11 +985,104 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint } } +float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos) +{ + if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) { + return global_pos.terrain_alt; + } + + return takeoff_alt; +} + +bool FixedwingPositionControl::update_desired_altitude(float dt) +{ + /* + * The complete range is -1..+1, so this is 6% + * of the up or down range or 3% of the total range. + */ + const float deadBand = 0.06f; + + /* + * The correct scaling of the complete range needs + * to account for the missing part of the slope + * due to the deadband + */ + const float factor = 1.0f - deadBand; + + /* Climbout mode sets maximum throttle and pitch up */ + bool climbout_mode = false; + + /* + * Reset the hold altitude to the current altitude if the uncertainty + * changes significantly. + * This is to guard against uncommanded altitude changes + * when the altitude certainty increases or decreases. + */ + + if (fabsf(_althold_epv - _global_pos.epv) > ALTHOLD_EPV_RESET_THRESH) { + _hold_alt = _global_pos.alt; + _althold_epv = _global_pos.epv; + } + + /* + * Manual control has as convention the rotation around + * an axis. Positive X means to rotate positively around + * the X axis in NED frame, which is pitching down + */ + if (_manual.x > deadBand) { + /* pitching down */ + float pitch = -(_manual.x - deadBand) / factor; + _hold_alt += (_parameters.max_sink_rate * dt) * pitch; + _was_in_deadband = false; + } else if (_manual.x < - deadBand) { + /* pitching up */ + float pitch = -(_manual.x + deadBand) / factor; + _hold_alt += (_parameters.max_climb_rate * dt) * pitch; + _was_in_deadband = false; + climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH); + } else if (!_was_in_deadband) { + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ + _hold_alt = _global_pos.alt; + _althold_epv = _global_pos.epv; + _was_in_deadband = true; + } + if (_vehicle_status.is_vtol) { + if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) { + _hold_alt = _global_pos.alt; + } + } + return climbout_mode; +} + +bool FixedwingPositionControl::in_takeoff_situation() { + const hrt_abstime delta_takeoff = 10000000; + const float throttle_threshold = 0.1f; + + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) { + return true; + } + + return false; +} + +void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_limit_min) +{ + /* demand "climbout_diff" m above ground if user switched into this mode during takeoff */ + if (in_takeoff_situation()) { + *hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff; + *pitch_limit_min = math::radians(10.0f); + } else { + *pitch_limit_min = _parameters.pitch_limit_min; + } +} + bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - float dt = FLT_MIN; // Using non zero value to a avoid division by zero + float dt = 0.01; // Using non zero value to a avoid division by zero if (_control_position_last_called > 0) { dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f; } @@ -893,25 +1090,46 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; - math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; - calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); - + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.apply_flaps = false; // by default we don't use flaps float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; + /* tell TECS to update its state, but let it know when it cannot actually control the plane */ + bool in_air_alt_control = (!_vehicle_status.condition_landed && + (_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_altitude_enabled)); + + /* update TECS filters */ if (!_mTecs.getEnabled()) { - _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, + accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); } + math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; + calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ float throttle_max = 1.0f; + /* save time when airplane is in air */ + if (!_was_in_air && !_vehicle_status.condition_landed) { + _was_in_air = true; + _time_went_in_air = hrt_absolute_time(); + _takeoff_ground_alt = _global_pos.alt; + } + /* reset flag when airplane landed */ + if (_vehicle_status.condition_landed) { + _was_in_air = false; + } + if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { /* AUTONOMOUS FLIGHT */ @@ -921,13 +1139,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { + _tecs.reset_state(); } } _control_mode_current = FW_POSCTRL_MODE_AUTO; /* reset hold altitude */ _hold_alt = _global_pos.alt; + /* reset hold yaw */ + _hdg_hold_yaw = _yaw; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -963,7 +1185,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _att_sp.thrust = 0.0f; + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; + + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); @@ -982,13 +1209,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + float alt_sp; + if (_nav_capabilities.abort_landing == true) { + // if we entered loiter due to an aborted landing, demand + // altitude setpoint well above landing waypoint + alt_sp = _pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff; + } else { + // use altitude given by wapoint + alt_sp = _pos_sp_triplet.current.alt; + } + + if (in_takeoff_situation() || + ((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff ) && _nav_capabilities.abort_landing == true)) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + } + + tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + // apply full flaps for landings. this flag will also trigger the use of flaperons + // if they have been enabled using the corresponding parameter + _att_sp.apply_flaps = true; + + // save time at which we started landing + if (_time_started_landing == 0) { + _time_started_landing = hrt_absolute_time(); + } + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -1001,7 +1254,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi wp_distance_save = 0.0f; } - //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + // create virtual waypoint which is on the desired flight path but + // some distance behind landing waypoint. This will make sure that the plane + // will always follow the desired flight path even if we get close or past + // the landing waypoint + math::Vector<2> curr_wp_shifted; + double lat; + double lon; + create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); + curr_wp_shifted(0) = (float)lat; + curr_wp_shifted(1) = (float)lon; + + // we want the plane to keep tracking the desired flight path until we start flaring + // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds + //if (land_noreturn_vertical) { if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -1010,17 +1276,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (pos_sp_triplet.previous.valid) { target_bearing = bearing_lastwp_currwp; } else { - target_bearing = _att.yaw; + target_bearing = _yaw; } - mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); + mavlink_log_info(_mavlink_fd, "#Landing, heading hold"); } -// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); - - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); - /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + _l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d); land_noreturn_horizontal = true; @@ -1033,6 +1296,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + if (land_noreturn_horizontal) { + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + } /* Vertical landing control */ //xxx: using the tecs altitude controller for slope control for now @@ -1040,12 +1307,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this could make a great param float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = 1.3f * _parameters.airspeed_min; - float airspeed_approach = 1.3f * _parameters.airspeed_min; + float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min; + float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be * equal to _pos_sp_triplet.current.alt */ - float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos); + float terrain_alt; + if (_parameters.land_use_terrain_estimate) { + if (_global_pos.terrain_alt_valid) { + // all good, have valid terrain altitude + terrain_alt = _global_pos.terrain_alt; + _t_alt_prev_valid = terrain_alt; + _time_last_t_alt = hrt_absolute_time(); + } else if (_time_last_t_alt == 0) { + // we have started landing phase but don't have valid terrain + // wait for some time, maybe we will soon get a valid estimate + // until then just use the altitude of the landing waypoint + if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { + terrain_alt = _pos_sp_triplet.current.alt; + } else { + // still no valid terrain, abort landing + terrain_alt = _pos_sp_triplet.current.alt; + _nav_capabilities.abort_landing = true; + } + } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) + || land_noreturn_vertical) { + // use previous terrain estimate for some time and hope to recover + // if we are already flaring (land_noreturn_vertical) then just + // go with the old estimate + terrain_alt = _t_alt_prev_valid; + } else { + // terrain alt was not valid for long time, abort landing + terrain_alt = _t_alt_prev_valid; + _nav_capabilities.abort_landing = true; + } + } else { + // no terrain estimation, just use landing waypoint altitude + terrain_alt = _pos_sp_triplet.current.alt; + } + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? @@ -1068,11 +1368,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; + /* enable direct yaw control using rudder/wheel */ + _att_sp.yaw_body = target_bearing; + _att_sp.fw_control_yaw = true; + if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; - mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle"); + mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle"); } } @@ -1096,10 +1400,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); + // just started with the flaring phase + _att_sp.pitch_body = 0.0f; + height_flare = _global_pos.alt - terrain_alt; + mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; + } else { + if (_global_pos.vel_d > 0.1f) { + _att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) * + math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); + } else { + _att_sp.pitch_body = _att_sp.pitch_body; + } } - //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_rel_last = flare_curve_alt_rel; } else { @@ -1118,7 +1431,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); + mavlink_log_info(_mavlink_fd, "#Landing, on slope"); land_onslope = true; } } else { @@ -1142,94 +1455,160 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - /* Perform launch detection */ - if (launchDetector.launchDetectionEnabled() && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - /* Inform user that launchdetection is running */ - static hrt_abstime last_sent = 0; - if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); - last_sent = hrt_absolute_time(); - } + if (_runway_takeoff.runwayTakeoffEnabled()) { + if (!_runway_takeoff.isInitialized()) { + math::Quaternion q(&_ctrl_state.q[0]); + math::Vector<3> euler = q.to_euler(); + _runway_takeoff.init(euler(2), _global_pos.lat, _global_pos.lon); - /* Detect launch */ - launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + /* need this already before takeoff is detected + * doesn't matter if it gets reset when takeoff is detected eventually */ + _takeoff_ground_alt = _global_pos.alt; - /* update our copy of the laucn detection state */ - launch_detection_state = launchDetector.getLaunchDetected(); - } else { - /* no takeoff detection --> fly */ - launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; - } + mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); + } - /* Set control values depending on the detection state */ - if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { - /* Launch has been detected, hence we have to control the plane. */ + float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + // update runway takeoff helper + _runway_takeoff.update( + _ctrl_state.airspeed, + _global_pos.alt - terrain_alt, + _global_pos.lat, + _global_pos.lon, + _mavlink_fd); - /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use - * full throttle, otherwise we use the preTakeOff Throttle */ - float takeoff_throttle = launch_detection_state != - LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? - launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; + /* + * Update navigation: _runway_takeoff returns the start WP according to mode and phase. + * If we use the navigator heading or not is decided later. + */ + _l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d); - /* select maximum pitch: the launchdetector may impose another limit for the pitch - * depending on the state of the launch */ - float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + // update tecs + float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - /* apply minimum pitch and limit roll if target altitude is not within climbout_diff - * meters */ - if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { - - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), - eas2tas, - math::radians(_parameters.pitch_limit_min), - takeoff_pitch_max_rad, - _parameters.throttle_min, takeoff_throttle, - _parameters.throttle_cruise, - true, - math::max(math::radians(pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed( + _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? + _parameters.throttle_cruise, + _runway_takeoff.climbout(), + math::radians(_runway_takeoff.getMinPitch( + _pos_sp_triplet.current.pitch_min, + 10.0f, + _parameters.pitch_limit_min)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); + + // assign values + _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); + _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); + _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + // reset integrals except yaw (which also counts for the wheel controller) + _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); + + /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, + (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ + } else { + /* Perform launch detection */ + if (launchDetector.launchDetectionEnabled() && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + /* Inform user that launchdetection is running */ + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { + mavlink_log_critical(_mavlink_fd, "#Launchdetection running"); + last_sent = hrt_absolute_time(); + } + + /* Detect launch */ + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + + /* update our copy of the launch detection state */ + launch_detection_state = launchDetector.getLaunchDetected(); + } else { + /* no takeoff detection --> fly */ + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } + + /* Set control values depending on the detection state */ + if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { + /* Launch has been detected, hence we have to control the plane. */ + + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use + * full throttle, otherwise we use the preTakeOff Throttle */ + float takeoff_throttle = launch_detection_state != + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? + launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; + + /* select maximum pitch: the launchdetector may impose another limit for the pitch + * depending on the state of the launch */ + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff + * meters */ + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_cruise, + true, + math::max(math::radians(_pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); + + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + + } else { + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + takeoff_throttle, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); + } } else { - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(_parameters.airspeed_trim), - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - takeoff_throttle, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Set default roll and pitch setpoints during detection phase */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)); } - } else { - /* Tell the attitude controller to stop integrating while we are waiting - * for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; - - /* Set default roll and pitch setpoints during detection phase */ - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), - math::radians(10.0f)); } } @@ -1251,20 +1630,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode.flag_control_velocity_enabled && _control_mode.flag_control_altitude_enabled) { - /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ + /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed, + heading is set to a distant waypoint */ - const float deadBand = (60.0f/1000.0f); - const float factor = 1.0f - deadBand; if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; + _hdg_hold_yaw = _yaw; + _hdg_hold_enabled = false; // this makes sure the waypoints are reset below + _yaw_lock_engaged = false; } /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { + _tecs.reset_state(); } } _control_mode_current = FW_POSCTRL_MODE_POSITION; @@ -1274,33 +1657,144 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; - /* Get demanded vertical velocity from pitch control */ - static bool was_in_deadband = false; - if (_manual.x > deadBand) { - float pitch = (_manual.x - deadBand) / factor; - _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; - was_in_deadband = false; - } else if (_manual.x < - deadBand) { - float pitch = (_manual.x + deadBand) / factor; - _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; - was_in_deadband = false; - } else if (!was_in_deadband) { - /* store altitude at which manual.x was inside deadBand - * The aircraft should immediately try to fly at this altitude - * as this is what the pilot expects when he moves the stick to the center */ - _hold_alt = _global_pos.alt; - was_in_deadband = true; + /* update desired altitude based on user pitch stick input */ + bool climbout_requested = update_desired_altitude(dt); + + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + * and set limit to pitch angle to prevent stearing into ground + */ + float pitch_limit_min; + do_takeoff_help(&_hold_alt, &pitch_limit_min); + + + /* throttle limiting */ + throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < THROTTLE_THRESH) { + throttle_max = 0.0f; } + tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, - _parameters.throttle_max, + throttle_max, _parameters.throttle_cruise, - false, + climbout_requested, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_NORMAL); + + /* heading control */ + + if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) { + /* heading / roll is zero, lock onto current heading */ + if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + // little yaw movement, lock to current heading + _yaw_lock_engaged = true; + + } + + /* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration + to make sure the plane does not start rolling + */ + if (in_takeoff_situation()) { + _hdg_hold_enabled = false; + _yaw_lock_engaged = true; + } + + if (_yaw_lock_engaged) { + + /* just switched back from non heading-hold to heading hold */ + if (!_hdg_hold_enabled) { + _hdg_hold_enabled = true; + _hdg_hold_yaw = _yaw; + + get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); + } + + /* we have a valid heading hold position, are we too close? */ + if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { + get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + } + + math::Vector<2> prev_wp; + prev_wp(0) = (float)_hdg_hold_prev_wp.lat; + prev_wp(1) = (float)_hdg_hold_prev_wp.lon; + + math::Vector<2> curr_wp; + curr_wp(0) = (float)_hdg_hold_curr_wp.lat; + curr_wp(1) = (float)_hdg_hold_curr_wp.lon; + + /* populate l1 control setpoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + if (in_takeoff_situation()) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + } + } + } else { + _hdg_hold_enabled = false; + _yaw_lock_engaged = false; + } + + } else if (_control_mode.flag_control_altitude_enabled) { + /* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ + + if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) { + /* Need to init because last loop iteration was in a different mode */ + _hold_alt = _global_pos.alt; + } + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { + _tecs.reset_state(); + } + } + _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; + + /* Get demanded airspeed */ + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; + + /* update desired altitude based on user pitch stick input */ + bool climbout_requested = update_desired_altitude(dt); + + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + * and set limit to pitch angle to prevent stearing into ground + */ + float pitch_limit_min; + do_takeoff_help(&_hold_alt, &pitch_limit_min); + + /* throttle limiting */ + throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < THROTTLE_THRESH) { + throttle_max = 0.0f; + } + + tecs_update_pitch_throttle(_hold_alt, + altctrl_airspeed, + eas2tas, math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + throttle_max, + _parameters.throttle_cruise, + climbout_requested, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); @@ -1309,9 +1803,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /** MANUAL FLIGHT **/ - /* reset hold altitude */ + // reset hold altitude _hold_alt = _global_pos.alt; + // reset terrain estimation relevant values + _time_started_landing = 0; + _time_last_t_alt = 0; + + // reset lading abort state + _nav_capabilities.abort_landing = false; + /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; @@ -1326,23 +1827,50 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && + !_runway_takeoff.runwayTakeoffEnabled()) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _runway_takeoff.runwayTakeoffEnabled()) { + _att_sp.thrust = _runway_takeoff.getThrottle( + math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max)); + + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _att_sp.thrust = 0.0f; + } else { /* Copy thrust and pitch values from tecs */ - _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : - _tecs.get_throttle_demand(), throttle_max); + if (_vehicle_status.condition_landed && + (_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE)) + { + // when we are landed in these modes we want the motor to spin + _att_sp.thrust = math::min(TAKEOFF_IDLE, throttle_max); + } else { + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max); + } + + } /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ - if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && ( + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + (launch_detection_state == LAUNCHDETECTION_RES_NONE || + _runway_takeoff.runwayTakeoffEnabled())) || + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && + land_noreturn_vertical)) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } @@ -1365,11 +1893,10 @@ FixedwingPositionControl::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -1388,7 +1915,7 @@ FixedwingPositionControl::task_main() } /* wakeup source(s) */ - struct pollfd fds[2]; + px4_pollfd_struct_t fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -1401,11 +1928,12 @@ FixedwingPositionControl::task_main() while (!_task_should_exit) { /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -1436,7 +1964,7 @@ FixedwingPositionControl::task_main() /* XXX Hack to get mavlink output going */ if (_mavlink_fd < 0) { /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); } /* load local copies */ @@ -1445,10 +1973,9 @@ FixedwingPositionControl::task_main() // XXX add timestamp check _global_pos_valid = true; - vehicle_attitude_poll(); + control_state_poll(); vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); - vehicle_airspeed_poll(); vehicle_manual_control_setpoint_poll(); // vehicle_baro_poll(); @@ -1463,11 +1990,11 @@ FixedwingPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr) { + if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); - } else { + } else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } @@ -1476,7 +2003,8 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { + if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON + && turn_distance > 0)) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; @@ -1496,13 +2024,13 @@ FixedwingPositionControl::task_main() warnx("exiting.\n"); _control_task = -1; - _exit(0); } void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; launchDetector.reset(); + _runway_takeoff.reset(); } void FixedwingPositionControl::reset_landing_state() @@ -1523,6 +2051,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, unsigned mode, bool pitch_max_special) { + /* do not run tecs if we are not in air */ + if (_vehicle_status.condition_landed) { + return; + } + if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ float flightPathAngle = 0.0f; @@ -1549,7 +2082,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ /* use pitch max set by MT param */ limitOverride.disablePitchMaxOverride(); } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode, limitOverride); } else { if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { @@ -1562,8 +2095,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* Using tecs library */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, - _airspeed.indicated_airspeed_m_s, eas2tas, + _tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp, + _ctrl_state.airspeed, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); @@ -1590,22 +2123,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ break; } - t.altitudeSp = s.hgt_dem; - t.altitude_filtered = s.hgt; - t.airspeedSp = s.spd_dem; - t.airspeed_filtered = s.spd; + t.altitudeSp = s.altitude_sp; + t.altitude_filtered = s.altitude_filtered; + t.airspeedSp = s.airspeed_sp; + t.airspeed_filtered = s.airspeed_filtered; - t.flightPathAngleSp = s.dhgt_dem; - t.flightPathAngle = s.dhgt; - t.flightPathAngleFiltered = s.dhgt; + t.flightPathAngleSp = s.altitude_rate_sp; + t.flightPathAngle = s.altitude_rate; + t.flightPathAngleFiltered = s.altitude_rate; - t.airspeedDerivativeSp = s.dspd_dem; - t.airspeedDerivative = s.dspd; + t.airspeedDerivativeSp = s.airspeed_rate_sp; + t.airspeedDerivative = s.airspeed_rate; - t.totalEnergyRateSp = s.thr; - t.totalEnergyRate = s.ithr; - t.energyDistributionRateSp = s.ptch; - t.energyDistributionRate = s.iptch; + t.totalEnergyError = s.total_energy_error; + t.totalEnergyRateError = s.total_energy_rate_error; + t.energyDistributionError = s.energy_distribution_error; + t.energyDistributionRateError = s.energy_distribution_rate_error; + + t.throttle_integ = s.throttle_integ; + t.pitch_integ = s.pitch_integ; if (_tecs_status_pub != nullptr) { orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); @@ -1624,8 +2160,8 @@ FixedwingPositionControl::start() _control_task = px4_task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 1600, - (main_t)&FixedwingPositionControl::task_main_trampoline, + 1300, + (px4_main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -1639,16 +2175,20 @@ FixedwingPositionControl::start() int fw_pos_control_l1_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: fw_pos_control_l1 {start|stop|status}"); + warnx("usage: fw_pos_control_l1 {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { - if (l1_control::g_control != nullptr) - errx(1, "already running"); + if (l1_control::g_control != nullptr) { + warnx("already running"); + return 1; + } if (OK != FixedwingPositionControl::start()) { - err(1, "start failed"); + warn("start failed"); + return 1; } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ @@ -1659,24 +2199,28 @@ int fw_pos_control_l1_main(int argc, char *argv[]) } printf("\n"); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { - if (l1_control::g_control == nullptr) - errx(1, "not running"); + if (l1_control::g_control == nullptr){ + warnx("not running"); + return 1; + } delete l1_control::g_control; l1_control::g_control = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (l1_control::g_control) { - errx(0, "running"); + warnx("running"); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5720d7b57cb4..14b73038a0ad 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -39,9 +39,6 @@ * @author Lorenz Meier */ -#include -#include - /* * Controller parameters, accessible via MAVLink */ @@ -58,7 +55,7 @@ * @max 100.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); +PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f); /** * L1 damping @@ -80,7 +77,7 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); * @max 1.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); +PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.6f); /** * Throttle max slew rate @@ -123,10 +120,11 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); * The maximum roll the controller will output. * * @unit degrees - * @min 0.0 + * @min 35.0 + * @max 65.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); +PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); /** * Throttle limit max @@ -151,6 +149,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); * For aircraft with internal combustion engine this parameter should be set * for desired idle rpm. * + * @min 0.0 + * @max 1.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); @@ -161,6 +161,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. * + * @min 0.0 + * @max 1.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); @@ -173,9 +175,11 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); * distance to the desired altitude. Mostly used for takeoff waypoints / modes. * Set to zero to disable climbout mode (not recommended). * + * @min 0.0 + * @max 150.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); +PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f); /** * Maximum climb rate @@ -193,6 +197,8 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or * FW_THR_MAX reduced. * + * @min 2.0 + * @max 10.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); @@ -318,7 +324,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); * * @group Fixed Wing TECS */ -PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); +PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); /** * Speed <--> Altitude priority @@ -369,7 +375,7 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f); * * @group Fixed Wing TECS */ -PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f); /** * Landing slope angle @@ -437,3 +443,15 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); + +/** + * Takeoff and landing airspeed scale factor + * + * Multiplying this factor with the minimum airspeed of the plane + * gives the target airspeed for takeoff and landing approach. + * + * @min 1.0 + * @max 1.5 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_SCALE, 1.3f); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index a855e4092ff4..7a045fb1bb15 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -52,15 +52,17 @@ mTecs::mTecs() : _mTecsEnabled(this, "ENABLED"), _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ - _status(ORB_ID(tecs_status), &getPublications()), +#if defined(__PX4_NUTTX) + _status(ORB_ID(tecs_status), -1, &getPublications()), +#endif // defined(__PX4_NUTTX) /* control blocks */ _controlTotalEnergy(this, "THR"), _controlEnergyDistribution(this, "PIT", true), _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), - _flightPathAngleLowpass(this, "FPA_LP"), - _altitudeLowpass(this, "ALT_LP"), - _airspeedLowpass(this, "A_LP"), + _flightPathAngleLowpass(this, "FPA_LP", 50), + _altitudeLowpass(this, "ALT_LP", 50), + _airspeedLowpass(this, "A_LP", 50), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), _pitchSp(0.0f), @@ -86,8 +88,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ - if (!isfinite(flightPathAngle) || !isfinite(altitude) || - !isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) || + !PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { return -1; } @@ -107,9 +109,11 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp); } +#if defined(__PX4_NUTTX) /* Write part of the status message */ - _status.altitudeSp = altitudeSp; - _status.altitude_filtered = altitudeFiltered; + _status.get().altitudeSp = altitudeSp; + _status.get().altitude_filtered = altitudeFiltered; +#endif // defined(__PX4_NUTTX) /* use flightpath angle setpoint for total energy control */ @@ -121,8 +125,8 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ - if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || - !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || + !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { return -1; } @@ -143,9 +147,11 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng (double)airspeedFiltered, (double)accelerationLongitudinalSp); } +#if defined(__PX4_NUTTX) /* Write part of the status message */ - _status.airspeedSp = airspeedSp; - _status.airspeed_filtered = airspeedFiltered; + _status.get().airspeedSp = airspeedSp; + _status.get().airspeed_filtered = airspeedFiltered; +#endif // defined(__PX4_NUTTX) /* use longitudinal acceleration setpoint for total energy control */ @@ -157,8 +163,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight float accelerationLongitudinalSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ - if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || - !isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || + !PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) { return -1; } /* time measurement */ @@ -200,43 +206,45 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight } /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ - if (mode != tecs_status_s::TECS_MODE_LAND && mode != tecs_status_s::TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) { - mode = tecs_status_s::TECS_MODE_UNDERSPEED; + if (mode != MTECS_MODE_LAND && mode != MTECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) { + mode = MTECS_MODE_UNDERSPEED; } - /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ + /* Set special output limiters if we are not in MTECS_MODE_NORMAL */ BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); - if (mode == tecs_status_s::TECS_MODE_TAKEOFF) { + if (mode == MTECS_MODE_TAKEOFF) { outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; - } else if (mode == tecs_status_s::TECS_MODE_LAND) { + } else if (mode == MTECS_MODE_LAND) { // only limit pitch but do not limit throttle outputLimiterPitch = &_BlockOutputLimiterLandPitch; - } else if (mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM) { + } else if (mode == MTECS_MODE_LAND_THROTTLELIM) { outputLimiterThrottle = &_BlockOutputLimiterLandThrottle; outputLimiterPitch = &_BlockOutputLimiterLandPitch; - } else if (mode == tecs_status_s::TECS_MODE_UNDERSPEED) { + } else if (mode == MTECS_MODE_UNDERSPEED) { outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; } - /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by + /* Apply override given by the limitOverride argument (this is used for limits which are not given by * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector * is running) */ limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); - /* Write part of the status message */ - _status.flightPathAngleSp = flightPathAngleSp; - _status.flightPathAngle = flightPathAngle; - _status.flightPathAngleFiltered = flightPathAngleFiltered; - _status.airspeedDerivativeSp = airspeedDerivativeSp; - _status.airspeedDerivative = airspeedDerivative; - _status.totalEnergyRateSp = totalEnergyRateSp; - _status.totalEnergyRate = totalEnergyRate; - _status.energyDistributionRateSp = energyDistributionRateSp; - _status.energyDistributionRate = energyDistributionRate; - _status.mode = mode; +// #if defined(__PX4_NUTTX) + // /* Write part of the status message */ + // _status.flightPathAngleSp = flightPathAngleSp; + // _status.flightPathAngle = flightPathAngle; + // _status.flightPathAngleFiltered = flightPathAngleFiltered; + // _status.airspeedDerivativeSp = airspeedDerivativeSp; + // _status.airspeedDerivative = airspeedDerivative; + // _status.totalEnergyRateSp = totalEnergyRateSp; + // _status.totalEnergyRate = totalEnergyRate; + // _status.energyDistributionRateSp = energyDistributionRateSp; + // _status.energyDistributionRate = energyDistributionRate; + // _status.mode = mode; +// #endif // defined(__PX4_NUTTX) /** update control blocks **/ /* update total energy rate control block */ @@ -253,8 +261,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight (double)accelerationLongitudinalSp, (double)airspeedDerivative); } - /* publish status messge */ +#if defined(__PX4_NUTTX) + /* publish status message */ _status.update(); +#endif // defined(__PX4_NUTTX) /* clean up */ _firstIterationAfterReset = false; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index e114cc3ae018..09d9eec1d577 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -49,11 +49,25 @@ #include #include #include + +#if defined(__PX4_NUTTX) #include +#endif // defined(__PX4_NUTTX) namespace fwPosctrl { +/* corresponds to TECS_MODE in tecs_status.msg */ +enum MTECS_MODE { + MTECS_MODE_NORMAL = 0, + MTECS_MODE_UNDERSPEED = 1, + MTECS_MODE_TAKEOFF = 2, + MTECS_MODE_LAND = 3, + MTECS_MODE_LAND_THROTTLELIM = 4, + MTECS_MODE_BAD_DESCENT = 5, + MTECS_MODE_CLIMBOUT = 6 +}; + /* Main class of the mTecs */ class mTecs : public control::SuperBlock { @@ -94,6 +108,10 @@ class mTecs : public control::SuperBlock float getThrottleSetpoint() { return _throttleSp; } float getPitchSetpoint() { return _pitchSp; } float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); } + float getFlightPathAngleLowpassState() { return _flightPathAngleLowpass.getState(); } + float getAltitudeLowpassState() { return _altitudeLowpass.getState(); } + float getAirspeedLowpassState() { return _airspeedLowpass.getState(); } + float getAirspeedDerivativeLowpassState() { return _airspeedDerivative.getO(); } protected: /* parameters */ @@ -101,7 +119,9 @@ class mTecs : public control::SuperBlock control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ /* Publications */ +#if defined(__PX4_NUTTX) uORB::Publication _status; /**< publish internal values for logging */ +#endif // defined(__PX4_NUTTX) /* control blocks */ BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output @@ -114,9 +134,9 @@ class mTecs : public control::SuperBlock setpoint */ /* Other calculation Blocks */ - control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ - control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */ - control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ + control::BlockLowPass2 _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ + control::BlockLowPass2 _altitudeLowpass; /**< low pass filter for altitude */ + control::BlockLowPass2 _airspeedLowpass; /**< low pass filter for airspeed */ control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ /* Output setpoints */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 3b98454b954e..0d1025f7d42d 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -39,9 +39,6 @@ * @author Thomas Gubler */ -#include -#include - /* * Controller parameters, accessible via MAVLink */ diff --git a/src/modules/gpio_led/CMakeLists.txt b/src/modules/gpio_led/CMakeLists.txt new file mode 100644 index 000000000000..435d8e986af9 --- /dev/null +++ b/src/modules/gpio_led/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__gpio_led + MAIN gpio_led + COMPILE_FLAGS + -Os + SRCS + gpio_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 87fbfbf4aeb6..531a921b8348 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -208,12 +208,14 @@ void gpio_led_start(FAR void *arg) char *gpio_dev; #if defined(PX4IO_DEVICE_PATH) + if (priv->use_io) { gpio_dev = PX4IO_DEVICE_PATH; } else { gpio_dev = PX4FMU_DEVICE_PATH; } + #else gpio_dev = PX4FMU_DEVICE_PATH; #endif diff --git a/src/modules/land_detector/CMakeLists.txt b/src/modules/land_detector/CMakeLists.txt new file mode 100644 index 000000000000..3882f246c297 --- /dev/null +++ b/src/modules/land_detector/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__land_detector + MAIN land_detector + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + land_detector_main.cpp + LandDetector.cpp + MulticopterLandDetector.cpp + FixedwingLandDetector.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index e26954d1a626..f8066241c51a 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -40,6 +40,8 @@ #include "FixedwingLandDetector.h" +#include +#include #include #include @@ -47,14 +49,18 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle(), _params(), _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _airspeed({}), - _parameterSub(-1), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0) + _vehicleLocalPosition( {}), + _airspeedSub(-1), + _vehicleStatusSub(-1), + _armingSub(-1), + _airspeed{}, + _vehicleStatus{}, + _arming{}, + _parameterSub(-1), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); @@ -66,6 +72,8 @@ void FixedwingLandDetector::initialize() // Subscribe to local position and airspeed data _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + _vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status)); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); updateParameterCache(true); } @@ -74,6 +82,8 @@ void FixedwingLandDetector::updateSubscriptions() { orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); } bool FixedwingLandDetector::update() @@ -81,14 +91,32 @@ bool FixedwingLandDetector::update() // First poll for new data from our subscriptions updateSubscriptions(); + // only trigger flight conditions if we are armed + if (!_arming.armed) { + return true; + } + const uint64_t now = hrt_absolute_time(); bool landDetected = false; - // TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { + float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + + if (PX4_ISFINITE(val)) { + _velocity_xy_filtered = val; + } + + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); + + if (PX4_ISFINITE(val)) { + _velocity_z_filtered = val; + } + } + + if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + } // crude land detector for fixedwing if (_velocity_xy_filtered < _params.maxVelocity diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 0e9c092ee01b..325faee7944f 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -44,7 +44,9 @@ #include "LandDetector.h" #include #include +#include #include +#include #include class FixedwingLandDetector : public LandDetector @@ -90,11 +92,15 @@ class FixedwingLandDetector : public LandDetector } _params; private: - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; - int _parameterSub; + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + int _vehicleStatusSub; + int _armingSub; + struct airspeed_s _airspeed; + struct vehicle_status_s _vehicleStatus; + struct actuator_armed_s _arming; + int _parameterSub; float _velocity_xy_filtered; float _velocity_z_filtered; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index e4494ad56be1..3040e23407de 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -42,64 +42,77 @@ #include "LandDetector.h" #include //usleep #include +#include +#include LandDetector::LandDetector() : _landDetectedPub(0), - _landDetected({0, false}), - _taskShouldExit(false), - _taskIsRunning(false) -{ + _landDetected( {0, false}), + _arming_time(0), + _taskShouldExit(false), + _taskIsRunning(false), +_work{} { // ctor } LandDetector::~LandDetector() { + work_cancel(LPWORK, &_work); _taskShouldExit = true; } +int LandDetector::start() +{ + /* schedule a cycle to start things */ + work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); + + return 0; +} + void LandDetector::shutdown() { _taskShouldExit = true; } -void LandDetector::start() +void +LandDetector::cycle_trampoline(void *arg) { - // make sure this method has not already been called by another thread - if (isRunning()) { - return; - } - - // advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = (uintptr_t)orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + LandDetector *dev = reinterpret_cast(arg); - // initialize land detection algorithm - initialize(); - - // task is now running, keep doing so until shutdown() has been called - _taskIsRunning = true; - _taskShouldExit = false; - - while (isRunning()) { + dev->cycle(); +} - bool landDetected = update(); +void LandDetector::cycle() +{ + if (!_taskIsRunning) { + // advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + + // initialize land detection algorithm + initialize(); + + // task is now running, keep doing so until shutdown() has been called + _taskIsRunning = true; + _taskShouldExit = false; + } - // publish if land detection state has changed - if (_landDetected.landed != landDetected) { - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = landDetected; + bool landDetected = update(); - // publish the land detected broadcast - orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected); - } + // publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = landDetected; - // limit loop rate - usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); + // publish the land detected broadcast + orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected); } - _taskIsRunning = false; - _exit(0); + if (!_taskShouldExit) { + work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, + USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); + } } bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer) diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 4e1f30ca490c..b86e4311503c 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -41,6 +41,7 @@ #ifndef __LAND_DETECTOR_H__ #define __LAND_DETECTOR_H__ +#include #include #include @@ -70,7 +71,9 @@ class LandDetector * @brief Blocking function that should be called from it's own task thread. This method will * run the underlying algorithm at the desired update rate and publish if the landing state changes. **/ - void start(); + int start(); + + static void cycle_trampoline(void *arg); protected: @@ -87,7 +90,7 @@ class LandDetector virtual void initialize() = 0; /** - * @brief Convinience function for polling uORB subscriptions + * @brief Convenience function for polling uORB subscriptions * @return true if there was new data and it was successfully copied **/ bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); @@ -96,14 +99,20 @@ class LandDetector static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = + 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ protected: - uintptr_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + uint64_t _arming_time; /**< timestamp of arming time */ private: bool _taskShouldExit; /**< true if it is requested that this task should exit */ bool _taskIsRunning; /**< task has reached main loop and is currently running */ + struct work_s _work; + + void cycle(); }; #endif //__LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 1490232a4f41..fa636e496c10 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -53,12 +53,12 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _actuatorsSub(-1), _armingSub(-1), _parameterSub(-1), - _attitudeSub(-1), - _vehicleGlobalPosition({}), - _vehicleStatus({}), - _actuators({}), - _arming({}), - _vehicleAttitude({}), + _attitudeSub(-1), + _vehicleGlobalPosition{}, + _vehicleStatus{}, + _actuators{}, + _arming{}, + _vehicleAttitude{}, _landTimer(0) { _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); @@ -97,23 +97,47 @@ bool MulticopterLandDetector::update() // only trigger flight conditions if we are armed if (!_arming.armed) { + _arming_time = 0; return true; + + } else if (_arming_time == 0) { + _arming_time = hrt_absolute_time(); + } + + // return status based on armed state if no position lock is available + if (_vehicleGlobalPosition.timestamp == 0 || + hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) { + + // no position lock - not landed if armed + return !_arming.armed; } const uint64_t now = hrt_absolute_time(); - // check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate; + float armThresholdFactor = 1.0f; + + // Widen acceptance thresholds for landed state right after arming + // so that motor spool-up and other effects do not trigger false negatives + if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) { + armThresholdFactor = 2.5f; + } + + // check if we are moving vertically - this might see a spike after arming due to + // throttle-up vibration. If accelerating fast the throttle thresholds will still give + // an accurate in-air indication + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate * armThresholdFactor; // check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity - && _vehicleStatus.condition_global_position_valid; + && _vehicleStatus.condition_global_position_valid; // next look if all rotation angles are not moving - bool rotating = fabsf(_vehicleAttitude.rollspeed) > _params.maxRotation || - fabsf(_vehicleAttitude.pitchspeed) > _params.maxRotation || - fabsf(_vehicleAttitude.yawspeed) > _params.maxRotation; + float maxRotationScaled = _params.maxRotation * armThresholdFactor; + + bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) || + (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || + (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); // check if thrust output is minimal (about half of default) bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 8c57416b569a..d001be4e7f49 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -97,20 +97,20 @@ class MulticopterLandDetector : public LandDetector } _params; private: - int _vehicleGlobalPositionSub; /**< notification of global position */ + int _vehicleGlobalPositionSub; /**< notification of global position */ int _vehicleStatusSub; int _actuatorsSub; int _armingSub; int _parameterSub; int _attitudeSub; - struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ - struct vehicle_status_s _vehicleStatus; - struct actuator_controls_s _actuators; - struct actuator_armed_s _arming; - struct vehicle_attitude_s _vehicleAttitude; + struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ + struct vehicle_status_s _vehicleStatus; + struct actuator_controls_s _actuators; + struct actuator_armed_s _arming; + struct vehicle_attitude_s _vehicleAttitude; - uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ + uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ }; #endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 1ca319ce63ff..0aa6e1fad2da 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -38,6 +38,10 @@ * @author Johan Jansen */ +#include +#include +#include +#include #include //usleep #include #include @@ -63,48 +67,33 @@ extern "C" __EXPORT int land_detector_main(int argc, char *argv[]); //Private variables static LandDetector *land_detector_task = nullptr; -static int _landDetectorTaskID = -1; static char _currentMode[12]; -/** -* Deamon thread function -**/ -static void land_detector_deamon_thread(int argc, char *argv[]) -{ - land_detector_task->start(); -} - /** * Stop the task, force killing it if it doesn't stop by itself **/ static void land_detector_stop() { - if (land_detector_task == nullptr || _landDetectorTaskID == -1) { - errx(1, "not running"); + if (land_detector_task == nullptr) { + warnx("not running"); return; } land_detector_task->shutdown(); - //Wait for task to die + // Wait for task to die int i = 0; do { /* wait 20ms */ usleep(20000); - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_landDetectorTaskID); - break; - } - } while (land_detector_task->isRunning()); + } while (land_detector_task->isRunning() && ++i < 50); delete land_detector_task; land_detector_task = nullptr; - _landDetectorTaskID = -1; - errx(0, "land_detector has been stopped"); + warnx("land_detector has been stopped"); } /** @@ -112,8 +101,8 @@ static void land_detector_stop() **/ static int land_detector_start(const char *mode) { - if (land_detector_task != nullptr || _landDetectorTaskID != -1) { - errx(1, "already running"); + if (land_detector_task != nullptr) { + warnx("already running"); return -1; } @@ -125,26 +114,21 @@ static int land_detector_start(const char *mode) land_detector_task = new MulticopterLandDetector(); } else { - errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); + warnx("[mode] must be either 'fixedwing' or 'multicopter'"); return -1; } //Check if alloc worked if (land_detector_task == nullptr) { - errx(1, "alloc failed"); + warnx("alloc failed"); return -1; } //Start new thread task - _landDetectorTaskID = px4_task_spawn_cmd("land_detector", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1000, - (main_t)&land_detector_deamon_thread, - nullptr); - - if (_landDetectorTaskID < 0) { - errx(1, "task start failed: %d", -errno); + int ret = land_detector_task->start(); + + if (ret) { + warnx("task start failed: %d", -errno); return -1; } @@ -163,18 +147,18 @@ static int land_detector_start(const char *mode) usleep(50000); if (hrt_absolute_time() > timeout) { - err(1, "start failed - timeout"); + warnx("start failed - timeout"); land_detector_stop(); - exit(1); + return 1; } } + printf("\n"); } //Remember current active mode strncpy(_currentMode, mode, 12); - exit(0); return 0; } @@ -189,12 +173,17 @@ int land_detector_main(int argc, char *argv[]) } if (argc >= 2 && !strcmp(argv[1], "start")) { - land_detector_start(argv[2]); + if (land_detector_start(argv[2]) != 0) { + warnx("land_detector start failed"); + return 1; + } + + return 0; } if (!strcmp(argv[1], "stop")) { land_detector_stop(); - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { @@ -204,13 +193,14 @@ int land_detector_main(int argc, char *argv[]) warnx("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR"); } else { - errx(1, "exists, but not running (%s)", _currentMode); + warnx("exists, but not running (%s)", _currentMode); } - exit(0); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 9cf57b5fc9a5..d9201eb18a56 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -38,30 +38,34 @@ * @author Johan Jansen */ -#include - /** * Multicopter max climb rate * - * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * Maximum vertical velocity allowed in the landed state (m/s up and down) + * + * @unit m/s * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.70f); /** * Multicopter max horizontal velocity * - * Maximum horizontal velocity allowed to trigger a land (m/s) + * Maximum horizontal velocity allowed in the landed state (m/s) + * + * @unit m/s * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.50f); /** * Multicopter max rotation * - * Maximum allowed around each axis to trigger a land (degrees per second) + * Maximum allowed around each axis allowed in the landed state (degrees per second) + * + * @unit deg/s * * @group Land Detector */ @@ -70,35 +74,50 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); /** * Multicopter max throttle * - * Maximum actuator output on throttle before triggering a land + * Maximum actuator output on throttle allowed in the landed state + * + * @min 0.1 + * @max 0.5 * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.15f); /** * Fixedwing max horizontal velocity * - * Maximum horizontal velocity allowed to trigger a land (m/s) + * Maximum horizontal velocity allowed in the landed state (m/s) + * + * @min 0.5 + * @max 10 + * @unit m/s * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.40f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); /** * Fixedwing max climb rate * - * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * Maximum vertical velocity allowed in the landed state (m/s up and down) + * + * @min 5 + * @max 20 + * @unit m/s * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); /** * Airspeed max * - * Maximum airspeed allowed to trigger a land (m/s) + * Maximum airspeed allowed in the landed state (m/s) + * + * @min 4 + * @max 20 + * @unit m/s * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 8.00f); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp new file mode 100644 index 000000000000..d1262f9177d5 --- /dev/null +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -0,0 +1,1378 @@ +#include "BlockLocalPositionEstimator.hpp" +#include +#include +#include + +static const int MIN_FLOW_QUALITY = 100; +static const int REQ_INIT_COUNT = 100; + +static const uint32_t VISION_POSITION_TIMEOUT = 500000; +static const uint32_t MOCAP_TIMEOUT = 200000; + +static const uint32_t XY_SRC_TIMEOUT = 2000000; + +using namespace std; + +BlockLocalPositionEstimator::BlockLocalPositionEstimator() : + // this block has no parent, and has name LPE + SuperBlock(NULL, "LPE"), + + // subscriptions, set rate, add to list + // TODO topic speed limiting? + _sub_status(ORB_ID(vehicle_status), 0, 0, &getSubscriptions()), + _sub_armed(ORB_ID(actuator_armed), 0, 0, &getSubscriptions()), + _sub_control_mode(ORB_ID(vehicle_control_mode), + 0, 0, &getSubscriptions()), + _sub_att(ORB_ID(vehicle_attitude), 0, 0, &getSubscriptions()), + _sub_att_sp(ORB_ID(vehicle_attitude_setpoint), + 0, 0, &getSubscriptions()), + _sub_flow(ORB_ID(optical_flow), 0, 0, &getSubscriptions()), + _sub_sensor(ORB_ID(sensor_combined), 0, 0, &getSubscriptions()), + _sub_distance(ORB_ID(distance_sensor), + 0, 0, &getSubscriptions()), + _sub_param_update(ORB_ID(parameter_update), 0, 0, &getSubscriptions()), + _sub_manual(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()), + _sub_home(ORB_ID(home_position), 0, 0, &getSubscriptions()), + _sub_gps(ORB_ID(vehicle_gps_position), 0, 0, &getSubscriptions()), + _sub_vision_pos(ORB_ID(vision_position_estimate), 0, 0, &getSubscriptions()), + _sub_mocap(ORB_ID(att_pos_mocap), 0, 0, &getSubscriptions()), + + // publications + _pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()), + _pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()), + _pub_filtered_flow(ORB_ID(filtered_bottom_flow), -1, &getPublications()), + _pub_est_status(ORB_ID(estimator_status), -1, &getPublications()), + + // map projection + _map_ref(), + + // block parameters + _integrate(this, "INTEGRATE"), + _flow_xy_stddev(this, "FLW_XY"), + _sonar_z_stddev(this, "SNR_Z"), + _lidar_z_stddev(this, "LDR_Z"), + _accel_xy_stddev(this, "ACC_XY"), + _accel_z_stddev(this, "ACC_Z"), + _baro_stddev(this, "BAR_Z"), + _gps_xy_stddev(this, "GPS_XY"), + _gps_z_stddev(this, "GPS_Z"), + _gps_vxy_stddev(this, "GPS_VXY"), + _gps_vz_stddev(this, "GPS_VZ"), + _gps_eph_max(this, "EPH_MAX"), + _vision_xy_stddev(this, "VIS_XY"), + _vision_z_stddev(this, "VIS_Z"), + _no_vision(this, "NO_VISION"), + _beta_max(this, "BETA_MAX"), + _mocap_p_stddev(this, "VIC_P"), + _pn_p_noise_power(this, "PN_P"), + _pn_v_noise_power(this, "PN_V"), + _pn_b_noise_power(this, "PN_B"), + + // misc + _polls(), + _timeStamp(hrt_absolute_time()), + _time_last_xy(0), + _time_last_flow(0), + _time_last_baro(0), + _time_last_gps(0), + _time_last_lidar(0), + _time_last_sonar(0), + _time_last_vision_p(0), + _time_last_mocap(0), + + // mavlink log + _mavlink_fd(open(MAVLINK_LOG_DEVICE, 0)), + + // initialization flags + _baroInitialized(false), + _gpsInitialized(false), + _lidarInitialized(false), + _sonarInitialized(false), + _flowInitialized(false), + _visionInitialized(false), + _mocapInitialized(false), + + // init counts + _baroInitCount(0), + _gpsInitCount(0), + _lidarInitCount(0), + _sonarInitCount(0), + _flowInitCount(0), + _visionInitCount(0), + _mocapInitCount(0), + + // reference altitudes + _altHome(0), + _altHomeInitialized(false), + _baroAltHome(0), + _gpsAltHome(0), + _lidarAltHome(0), + _sonarAltHome(0), + _visionHome(), + _mocapHome(), + + // flow integration + _flowX(0), + _flowY(0), + _flowMeanQual(0), + + // reference lat/lon + _gpsLatHome(0), + _gpsLonHome(0), + + // status + _canEstimateXY(false), + _canEstimateZ(false), + _xyTimeout(false), + + // faults + _baroFault(FAULT_NONE), + _gpsFault(FAULT_NONE), + _lidarFault(FAULT_NONE), + _flowFault(FAULT_NONE), + _sonarFault(FAULT_NONE), + _visionFault(FAULT_NONE), + _mocapFault(FAULT_NONE), + + //timeouts + _visionTimeout(true), + _mocapTimeout(true), + + // loop performance + _loop_perf(), + _interval_perf(), + _err_perf(), + + // kf matrices + _x(), _u(), _P() +{ + // setup event triggering based on new flow messages to integrate + _polls[POLL_FLOW].fd = _sub_flow.getHandle(); + _polls[POLL_FLOW].events = POLLIN; + + _polls[POLL_PARAM].fd = _sub_param_update.getHandle(); + _polls[POLL_PARAM].events = POLLIN; + + _polls[POLL_SENSORS].fd = _sub_sensor.getHandle(); + _polls[POLL_SENSORS].events = POLLIN; + + // initialize P to identity*0.1 + initP(); + + _x.setZero(); + _u.setZero(); + + // perf counters + _loop_perf = perf_alloc(PC_ELAPSED, + "local_position_estimator_runtime"); + //_interval_perf = perf_alloc(PC_INTERVAL, + //"local_position_estimator_interval"); + _err_perf = perf_alloc(PC_COUNT, "local_position_estimator_err"); + + // map + _map_ref.init_done = false; + + // intialize parameter dependent matrices + updateParams(); +} + +BlockLocalPositionEstimator::~BlockLocalPositionEstimator() +{ +} + +void BlockLocalPositionEstimator::update() +{ + + // wait for a sensor update, check for exit condition every 100 ms + int ret = poll(_polls, 3, 100); + + if (ret < 0) { + /* poll error, count it in perf */ + perf_count(_err_perf); + return; + } + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + //printf("dt: %0.5g\n", double(dt)); + + // set dt for all child blocks + setDt(dt); + + // see which updates are available + bool flowUpdated = _sub_flow.updated(); + bool paramsUpdated = _sub_param_update.updated(); + bool baroUpdated = _sub_sensor.updated(); + bool lidarUpdated = false; + bool sonarUpdated = false; + + if (_sub_distance.updated()) { + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { + lidarUpdated = true; + } + + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { + sonarUpdated = true; + } + + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_INFRARED) { + mavlink_log_info(_mavlink_fd, "[lpe] no support to short-range infrared sensors "); + warnx("[lpe] short-range infrared detected. Ignored... "); + } + } + + bool gpsUpdated = _sub_gps.updated(); + bool homeUpdated = _sub_home.updated(); + bool visionUpdated = _sub_vision_pos.updated(); + bool mocapUpdated = _sub_mocap.updated(); + + // get new data + updateSubscriptions(); + + // update parameters + if (paramsUpdated) { + updateParams(); + } + + // update home position projection + if (homeUpdated) { + updateHome(); + } + + // check for timeouts on external sources + if ((hrt_absolute_time() - _time_last_vision_p > VISION_POSITION_TIMEOUT) && _visionInitialized) { + if (!_visionTimeout) { + _visionTimeout = true; + mavlink_log_info(_mavlink_fd, "[lpe] vision position timeout "); + warnx("[lpe] vision position timeout "); + } + + } else { + _visionTimeout = false; + } + + if ((hrt_absolute_time() - _time_last_mocap > MOCAP_TIMEOUT) && _mocapInitialized) { + if (!_mocapTimeout) { + _mocapTimeout = true; + mavlink_log_info(_mavlink_fd, "[lpe] mocap timeout "); + warnx("[lpe] mocap timeout "); + } + + } else { + _mocapTimeout = false; + } + + // determine if we should start estimating + _canEstimateZ = _baroInitialized && !_baroFault; + _canEstimateXY = + (_gpsInitialized && !_gpsFault) || + (_flowInitialized && !_flowFault) || + (_visionInitialized && !_visionTimeout && !_visionFault) || + (_mocapInitialized && !_mocapTimeout && !_mocapFault); + + if (_canEstimateXY) { + _time_last_xy = hrt_absolute_time(); + } + + // if we have no lat, lon initialized projection at 0,0 + if (_canEstimateXY && !_map_ref.init_done) { + map_projection_init(&_map_ref, 0, 0); + } + + // reinitialize x if necessary + bool reinit_x = false; + + for (int i = 0; i < n_x; i++) { + // should we do a reinit + // of sensors here? + // don't want it to take too long + if (!isfinite(_x(i))) { + reinit_x = true; + break; + } + } + + if (reinit_x) { + for (int i = 0; i < n_x; i++) { + _x(i) = 0; + } + + mavlink_log_info(_mavlink_fd, "[lpe] reinit x"); + warnx("[lpe] reinit x"); + } + + // reinitialize P if necessary + bool reinit_P = false; + + for (int i = 0; i < n_x; i++) { + for (int j = 0; j < n_x; j++) { + if (!isfinite(_P(i, j))) { + reinit_P = true; + break; + } + } + + if (reinit_P) { break; } + } + + if (reinit_P) { + mavlink_log_info(_mavlink_fd, "[lpe] reinit P"); + warnx("[lpe] reinit P"); + initP(); + } + + // do prediction + predict(); + + // sensor corrections/ initializations + if (gpsUpdated) { + if (!_gpsInitialized) { + initGps(); + + } else { + correctGps(); + } + } + + if (baroUpdated) { + if (!_baroInitialized) { + initBaro(); + + } else { + correctBaro(); + } + } + + if (lidarUpdated) { + if (!_lidarInitialized) { + initLidar(); + + } else { + correctLidar(); + } + } + + if (sonarUpdated) { + if (!_sonarInitialized) { + initSonar(); + + } else { + correctSonar(); + } + } + + if (flowUpdated) { + if (!_flowInitialized) { + initFlow(); + + } else { + perf_begin(_loop_perf);// TODO + correctFlow(); + //perf_count(_interval_perf); + perf_end(_loop_perf); + } + } + + if (_no_vision.get() != CBRK_NO_VISION_KEY) { // check if no vision circuit breaker is set + if (visionUpdated) { + if (!_visionInitialized) { + initVision(); + + } else { + correctVision(); + } + } + } + + if (mocapUpdated) { + if (!_mocapInitialized) { + initmocap(); + + } else { + correctmocap(); + } + } + + _xyTimeout = (hrt_absolute_time() - _time_last_xy > XY_SRC_TIMEOUT); + + if (!_xyTimeout && _altHomeInitialized) { + // update all publications if possible + publishLocalPos(); + publishEstimatorStatus(); + publishGlobalPos(); + publishFilteredFlow(); + + } else if (_altHomeInitialized) { + // publish only Z estimate + publishLocalPos(); + publishEstimatorStatus(); + } + +} + +void BlockLocalPositionEstimator::updateHome() +{ + double lat = _sub_home.get().lat; + double lon = _sub_home.get().lon; + float alt = _sub_home.get().alt; + + mavlink_log_info(_mavlink_fd, "[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", lat, lon, double(alt)); + warnx("[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", lat, lon, double(alt)); + map_projection_init(&_map_ref, lat, lon); + float delta_alt = alt - _altHome; + _altHomeInitialized = true; + _altHome = alt; + _gpsAltHome += delta_alt; + _baroAltHome += delta_alt; + _lidarAltHome += delta_alt; + _sonarAltHome += delta_alt; +} + +void BlockLocalPositionEstimator::initBaro() +{ + // collect baro data + if (!_baroInitialized && + (_sub_sensor.get().baro_timestamp[0] != _time_last_baro)) { + _time_last_baro = _sub_sensor.get().baro_timestamp[0]; + _baroAltHome += _sub_sensor.get().baro_alt_meter[0]; + + if (_baroInitCount++ > REQ_INIT_COUNT) { + _baroAltHome /= _baroInitCount; + mavlink_log_info(_mavlink_fd, + "[lpe] baro offs: %d m", (int)_baroAltHome); + warnx("[lpe] baro offs: %d m", (int)_baroAltHome); + _baroInitialized = true; + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _baroAltHome; + } + } + } +} + + +void BlockLocalPositionEstimator::initGps() +{ + // collect gps data + if (!_gpsInitialized && _sub_gps.get().fix_type > 2) { + double lat = _sub_gps.get().lat * 1e-7; + double lon = _sub_gps.get().lon * 1e-7; + float alt = _sub_gps.get().alt * 1e-3f; + // increament sums for mean + _gpsLatHome += lat; + _gpsLonHome += lon; + _gpsAltHome += alt; + _time_last_gps = _sub_gps.get().timestamp_position; + + if (_gpsInitCount++ > REQ_INIT_COUNT) { + _gpsLatHome /= _gpsInitCount; + _gpsLonHome /= _gpsInitCount; + _gpsAltHome /= _gpsInitCount; + map_projection_init(&_map_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[lpe] gps init: " + "lat %d, lon %d, alt %d m", + int(_gpsLatHome), int(_gpsLonHome), int(_gpsAltHome)); + warnx("[lpe] gps init: lat %d, lon %d, alt %d m", + int(_gpsLatHome), int(_gpsLonHome), int(_gpsAltHome)); + _gpsInitialized = true; + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _gpsAltHome; + } + } + } +} + +void BlockLocalPositionEstimator::initLidar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { return; } + + // collect lidar data + bool valid = false; + float d = _sub_distance.get().current_distance; + + if (d < _sub_distance.get().max_distance && + d > _sub_distance.get().min_distance) { + valid = true; + } + + if (!_lidarInitialized && valid) { + // increament sums for mean + _lidarAltHome += _sub_distance.get().current_distance; + + if (_lidarInitCount++ > REQ_INIT_COUNT) { + _lidarAltHome /= _lidarInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] lidar init: " + "alt %d cm", + int(100 * _lidarAltHome)); + warnx("[lpe] lidar init: alt %d cm", + int(100 * _lidarAltHome)); + _lidarInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initSonar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { return; } + + // collect sonar data + bool valid = false; + float d = _sub_distance.get().current_distance; + + if (d < _sub_distance.get().max_distance && + d > _sub_distance.get().min_distance) { + valid = true; + } + + if (!_sonarInitialized && valid) { + // increament sums for mean + _sonarAltHome += _sub_distance.get().current_distance; + + if (_sonarInitCount++ > REQ_INIT_COUNT) { + _sonarAltHome /= _sonarInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] sonar init: " + "alt %d cm", + int(100 * _sonarAltHome)); + warnx("[lpe] sonar init: alt %d cm", + int(100 * _sonarAltHome)); + _sonarInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initFlow() +{ + + // collect pixel flow data + if (!_flowInitialized) { + // increament sums for mean + _flowMeanQual += _sub_flow.get().quality; + + if (_flowInitCount++ > REQ_INIT_COUNT) { + _flowMeanQual /= _flowInitCount; + + if (_flowMeanQual < MIN_FLOW_QUALITY) { + // retry initialisation till we have better flow data + warnx("[lpe] flow quality bad, retrying init : %d", + int(_flowMeanQual)); + _flowMeanQual = 0; + _flowInitCount = 0; + return; + } + + mavlink_log_info(_mavlink_fd, "[lpe] flow init: " + "quality %d", + int(_flowMeanQual)); + warnx("[lpe] flow init: quality %d", + int(_flowMeanQual)); + _flowInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initVision() +{ + // collect vision position data + if (!_visionInitialized) { + // increament sums for mean + Vector3f pos; + pos(0) = _sub_vision_pos.get().x; + pos(1) = _sub_vision_pos.get().y; + pos(2) = _sub_vision_pos.get().z; + _visionHome += pos; + + if (_visionInitCount++ > REQ_INIT_COUNT) { + _visionHome /= _visionInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] vision position init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + warnx("[lpe] vision position init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + _visionInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initmocap() +{ + // collect mocap data + if (!_mocapInitialized) { + // increament sums for mean + Vector3f pos; + pos(0) = _sub_mocap.get().x; + pos(1) = _sub_mocap.get().y; + pos(2) = _sub_mocap.get().z; + _mocapHome += pos; + + if (_mocapInitCount++ > REQ_INIT_COUNT) { + _mocapHome /= _mocapInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] mocap init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + warnx("[lpe] mocap init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + _mocapInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::publishLocalPos() +{ + // publish local position + if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) + && isfinite(_x(X_vz))) { + _pub_lpos.get().timestamp = _timeStamp; + _pub_lpos.get().xy_valid = _canEstimateXY; + _pub_lpos.get().z_valid = _canEstimateZ; + _pub_lpos.get().v_xy_valid = _canEstimateXY; + _pub_lpos.get().v_z_valid = _canEstimateZ; + _pub_lpos.get().x = _x(X_x); // north + _pub_lpos.get().y = _x(X_y); // east + _pub_lpos.get().z = _x(X_z); // down + _pub_lpos.get().vx = _x(X_vx); // north + _pub_lpos.get().vy = _x(X_vy); // east + _pub_lpos.get().vz = _x(X_vz); // down + _pub_lpos.get().yaw = _sub_att.get().yaw; + _pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference + _pub_lpos.get().z_global = _baroInitialized; + _pub_lpos.get().ref_timestamp = _sub_home.get().timestamp; + _pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI; + _pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI; + _pub_lpos.get().ref_alt = _sub_home.get().alt; + // TODO, terrain alt + _pub_lpos.get().dist_bottom = -_x(X_z); + _pub_lpos.get().dist_bottom_rate = -_x(X_vz); + _pub_lpos.get().surface_bottom_timestamp = 0; + _pub_lpos.get().dist_bottom_valid = true; + _pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); + _pub_lpos.get().epv = sqrtf(_P(X_z, X_z)); + _pub_lpos.update(); + } +} + +void BlockLocalPositionEstimator::publishEstimatorStatus() +{ + if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) + && isfinite(_x(X_vz))) { + _pub_est_status.get().timestamp = _timeStamp; + + for (int i = 0; i < n_x; i++) { + _pub_est_status.get().states[i] = _x(i); + _pub_est_status.get().covariances[i] = _P(i, i); + } + + _pub_est_status.get().n_states = n_x; + _pub_est_status.get().nan_flags = 0; + _pub_est_status.get().health_flags = + ((_baroFault > 0) << SENSOR_BARO) + + ((_gpsFault > 0) << SENSOR_GPS) + + ((_lidarFault > 0) << SENSOR_LIDAR) + + ((_flowFault > 0) << SENSOR_FLOW) + + ((_sonarFault > 0) << SENSOR_SONAR) + + ((_visionFault > 0) << SENSOR_VISION) + + ((_mocapFault > 0) << SENSOR_MOCAP); + _pub_est_status.get().timeout_flags = + (_xyTimeout << 0) + + (_visionTimeout << 1) + + (_mocapTimeout << 2); + _pub_est_status.update(); + } +} + +void BlockLocalPositionEstimator::publishGlobalPos() +{ + // publish global position + double lat = 0; + double lon = 0; + map_projection_reproject(&_map_ref, _x(X_x), _x(X_y), &lat, &lon); + float alt = -_x(X_z) + _altHome; + + if (isfinite(lat) && isfinite(lon) && isfinite(alt) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) && + isfinite(_x(X_vz))) { + _pub_gpos.get().timestamp = _timeStamp; + _pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec; + _pub_gpos.get().lat = lat; + _pub_gpos.get().lon = lon; + _pub_gpos.get().alt = alt; + _pub_gpos.get().vel_n = _x(X_vx); + _pub_gpos.get().vel_e = _x(X_vy); + _pub_gpos.get().vel_d = _x(X_vz); + _pub_gpos.get().yaw = _sub_att.get().yaw; + _pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); + _pub_gpos.get().epv = sqrtf(_P(X_z, X_z)); + _pub_gpos.get().terrain_alt = 0; + _pub_gpos.get().terrain_alt_valid = false; + _pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout; + _pub_gpos.update(); + } +} + +void BlockLocalPositionEstimator::publishFilteredFlow() +{ + // publish filtered flow + if (isfinite(_pub_filtered_flow.get().sumx) && + isfinite(_pub_filtered_flow.get().sumy) && + isfinite(_pub_filtered_flow.get().vx) && + isfinite(_pub_filtered_flow.get().vy)) { + _pub_filtered_flow.update(); + } +} + +void BlockLocalPositionEstimator::initP() +{ + _P.setZero(); + _P(X_x, X_x) = 1; + _P(X_y, X_y) = 1; + _P(X_z, X_z) = 1; + _P(X_vx, X_vx) = 1; + _P(X_vy, X_vy) = 1; + _P(X_vz, X_vz) = 1; + _P(X_bx, X_bx) = 1e-6; + _P(X_by, X_by) = 1e-6; + _P(X_bz, X_bz) = 1e-6; +} + +void BlockLocalPositionEstimator::predict() +{ + // if can't update anything, don't propagate + // state or covariance + if (!_canEstimateXY && !_canEstimateZ) { return; } + + if (_integrate.get() && _sub_att.get().R_valid) { + Matrix3f R_att(_sub_att.get().R); + Vector3f a(_sub_sensor.get().accelerometer_m_s2); + _u = R_att * a; + _u(U_az) += 9.81f; // add g + + } else { + _u = Vector3f(0, 0, 0); + } + + // dynamics matrix + Matrix A; // state dynamics matrix + A.setZero(); + // derivative of position is velocity + A(X_x, X_vx) = 1; + A(X_y, X_vy) = 1; + A(X_z, X_vz) = 1; + + // derivative of velocity is accelerometer acceleration + // (in input matrix) - bias (in body frame) + Matrix3f R_att(_sub_att.get().R); + A(X_vx, X_bx) = -R_att(0, 0); + A(X_vx, X_by) = -R_att(0, 1); + A(X_vx, X_bz) = -R_att(0, 2); + + A(X_vy, X_bx) = -R_att(1, 0); + A(X_vy, X_by) = -R_att(1, 1); + A(X_vy, X_bz) = -R_att(1, 2); + + A(X_vz, X_bx) = -R_att(2, 0); + A(X_vz, X_by) = -R_att(2, 1); + A(X_vz, X_bz) = -R_att(2, 2); + + // input matrix + Matrix B; // input matrix + B.setZero(); + B(X_vx, U_ax) = 1; + B(X_vy, U_ay) = 1; + B(X_vz, U_az) = 1; + + // input noise covariance matrix + Matrix R; + R.setZero(); + R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); + R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); + R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get(); + + // process noise power matrix + Matrix Q; + Q.setZero(); + Q(X_x, X_x) = _pn_p_noise_power.get(); + Q(X_y, X_y) = _pn_p_noise_power.get(); + Q(X_z, X_z) = _pn_p_noise_power.get(); + Q(X_vx, X_vx) = _pn_v_noise_power.get(); + Q(X_vy, X_vy) = _pn_v_noise_power.get(); + Q(X_vz, X_vz) = _pn_v_noise_power.get(); + + // technically, the noise is in the body frame, + // but the components are all the same, so + // ignoring for now + Q(X_bx, X_bx) = _pn_b_noise_power.get(); + Q(X_by, X_by) = _pn_b_noise_power.get(); + Q(X_bz, X_bz) = _pn_b_noise_power.get(); + + // continuous time kalman filter prediction + Vector dx = (A * _x + B * _u) * getDt(); + + // only predict for components we have + // valid measurements for + if (!_canEstimateXY) { + dx(X_x) = 0; + dx(X_y) = 0; + dx(X_vx) = 0; + dx(X_vy) = 0; + } + + if (!_canEstimateZ) { + dx(X_z) = 0; + dx(X_vz) = 0; + } + + // propagate + _x += dx; + _P += (A * _P + _P * A.transpose() + + B * R * B.transpose() + Q) * getDt(); +} + +void BlockLocalPositionEstimator::correctFlow() +{ + + // flow measurement matrix and noise matrix + Matrix C; + C.setZero(); + C(Y_flow_x, X_x) = 1; + C(Y_flow_y, X_y) = 1; + + Matrix R; + R.setZero(); + R(Y_flow_x, Y_flow_x) = + _flow_xy_stddev.get() * _flow_xy_stddev.get(); + R(Y_flow_y, Y_flow_y) = + _flow_xy_stddev.get() * _flow_xy_stddev.get(); + + float flow_speed[3] = {0.0f, 0.0f, 0.0f}; + float global_speed[3] = {0.0f, 0.0f, 0.0f}; + + /* calc dt between flow timestamps */ + /* ignore first flow msg */ + if (_time_last_flow == 0) { + _time_last_flow = _sub_flow.get().timestamp; + return; + } + + float dt = (_sub_flow.get().timestamp - _time_last_flow) * 1.0e-6f ; + _time_last_flow = _sub_flow.get().timestamp; + + // calculate velocity over ground + if (_sub_flow.get().integration_timespan > 0) { + flow_speed[0] = (_sub_flow.get().pixel_flow_x_integral / + (_sub_flow.get().integration_timespan / 1e6f) - + _sub_att.get().pitchspeed) * // Body rotation correction TODO check this + _x(X_z); + flow_speed[1] = (_sub_flow.get().pixel_flow_y_integral / + (_sub_flow.get().integration_timespan / 1e6f) - + _sub_att.get().rollspeed) * // Body rotation correction + _x(X_z); + + } else { + flow_speed[0] = 0; + flow_speed[1] = 0; + } + + flow_speed[2] = 0.0f; + + /* update filtered flow */ + _pub_filtered_flow.get().sumx += flow_speed[0] * dt; + _pub_filtered_flow.get().sumy += flow_speed[1] * dt; + _pub_filtered_flow.get().vx = flow_speed[0]; + _pub_filtered_flow.get().vy = flow_speed[1]; + + // TODO add yaw rotation correction (with distance to vehicle zero) + + // convert to globalframe velocity + for (uint8_t i = 0; i < 3; i++) { + float sum = 0.0f; + + for (uint8_t j = 0; j < 3; j++) { + sum += flow_speed[j] * PX4_R(_sub_att.get().R, i, j); + } + + global_speed[i] = sum; + } + + // flow integral + _flowX += global_speed[0] * dt; + _flowY += global_speed[1] * dt; + + // measurement + Vector y; + y(0) = _flowX; + y(1) = _flowY; + + // residual + Vector r = y - C * _x; + + // residual covariance, (inverse) + Matrix S_I = + inv(C * _P * C.transpose() + R); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (_sub_flow.get().quality < MIN_FLOW_QUALITY) { + if (!_flowFault) { + mavlink_log_info(_mavlink_fd, "[lpe] bad flow data "); + warnx("[lpe] bad flow data "); + _flowFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_flowFault) { + mavlink_log_info(_mavlink_fd, "[lpe] flow fault, beta %5.2f", double(beta)); + warnx("[lpe] flow fault, beta %5.2f", double(beta)); + _flowFault = FAULT_MINOR; + } + + } else if (_flowFault) { + _flowFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] flow OK"); + warnx("[lpe] flow OK"); + } + + // kalman filter correction if no fault + if (_flowFault == FAULT_NONE) { + Matrix K = + _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + // reset flow integral to current estimate of position + // if a fault occurred + + } else { + _flowX = _x(X_x); + _flowY = _x(X_y); + } + +} + +void BlockLocalPositionEstimator::correctSonar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { + return; + } + + float d = _sub_distance.get().current_distance; + + // sonar measurement matrix and noise matrix + Matrix C; + C.setZero(); + C(Y_sonar_z, X_z) = -1; + + // use parameter covariance unless sensor provides reasonable value + Matrix R; + R.setZero(); + float cov = _sub_distance.get().covariance; + + if (cov < 1.0e-3f) { + R(0, 0) = _sonar_z_stddev.get() * _sonar_z_stddev.get(); + + } else { + R(0, 0) = cov; + } + + // measurement + Vector y; + y(0) = (d - _sonarAltHome) * + cosf(_sub_att.get().roll) * + cosf(_sub_att.get().pitch); + + // residual + Vector r = y - C * _x; + + // residual covariance, (inverse) + Matrix S_I = + inv(C * _P * C.transpose() + R); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (d < _sub_distance.get().min_distance || + d > _sub_distance.get().max_distance) { + if (!_sonarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] sonar out of range"); + warnx("[lpe] sonar out of range"); + _sonarFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_sonarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] sonar fault, beta %5.2f", double(beta)); + warnx("[lpe] sonar fault, beta %5.2f", double(beta)); + _sonarFault = FAULT_MINOR; + } + + } else if (_sonarFault) { + _sonarFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] sonar OK"); + warnx("[lpe] sonar OK"); + } + + // kalman filter correction if no fault + if (_sonarFault == FAULT_NONE) { + Matrix K = + _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_sonar = _sub_distance.get().timestamp; + +} + +void BlockLocalPositionEstimator::correctBaro() +{ + + Vector y; + y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome; + + // baro measurement matrix + Matrix C; + C.setZero(); + C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir. + + Matrix R; + R.setZero(); + R(0, 0) = _baro_stddev.get() * _baro_stddev.get(); + + // residual + Matrix S_I = + inv((C * _P * C.transpose()) + R); + Vector r = y - (C * _x); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_baroFault) { + mavlink_log_info(_mavlink_fd, "[lpe] baro fault, beta %5.2f", double(beta)); + warnx("[lpe] baro fault, beta %5.2f", double(beta)); + _baroFault = FAULT_MINOR; + } + + // lower baro trust + S_I = inv((C * _P * C.transpose()) + R * 10); + + } else if (_baroFault) { + _baroFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] baro OK"); + warnx("[lpe] baro OK"); + } + + // kalman filter correction if no fault + if (_baroFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_baro = _sub_sensor.get().baro_timestamp[0]; +} + +void BlockLocalPositionEstimator::correctLidar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { + return; + } + + float d = _sub_distance.get().current_distance; + + Matrix C; + C.setZero(); + C(Y_lidar_z, X_z) = -1; // measured altitude, + // negative down dir. + + // use parameter covariance unless sensor provides reasonable value + Matrix R; + R.setZero(); + float cov = _sub_distance.get().covariance; + + if (cov < 1.0e-3f) { + R(0, 0) = _lidar_z_stddev.get() * _lidar_z_stddev.get(); + + } else { + R(0, 0) = cov; + } + + Vector y; + y.setZero(); + y(0) = (d - _lidarAltHome) * + cosf(_sub_att.get().roll) * + cosf(_sub_att.get().pitch); + + // residual + Matrix S_I = inv((C * _P * C.transpose()) + R); + Vector r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + // zero is an error code for the lidar + if (d < _sub_distance.get().min_distance || + d > _sub_distance.get().max_distance) { + if (!_lidarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] lidar out of range"); + warnx("[lpe] lidar out of range"); + _lidarFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_lidarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] lidar fault, beta %5.2f", double(beta)); + warnx("[lpe] lidar fault, beta %5.2f", double(beta)); + _lidarFault = FAULT_MINOR; + } + + } else if (_lidarFault) { // disable fault if ok + _lidarFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] lidar OK"); + warnx("[lpe] lidar OK"); + } + + // kalman filter correction if no fault + if (_lidarFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_lidar = _sub_distance.get().timestamp; +} + +void BlockLocalPositionEstimator::correctGps() // TODO : use another other metric for glitch detection +{ + + // gps measurement in local frame + double lat = _sub_gps.get().lat * 1.0e-7; + double lon = _sub_gps.get().lon * 1.0e-7; + float alt = _sub_gps.get().alt * 1.0e-3f; + + float px = 0; + float py = 0; + float pz = alt - _gpsAltHome; + map_projection_project(&_map_ref, lat, lon, &px, &py); + + //printf("gps: lat %10g, lon, %10g alt %10g\n", lat, lon, double(alt)); + //printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt)); + //printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz)); + + Vector y; + y.setZero(); + y(0) = px; + y(1) = py; + y(2) = pz; + y(3) = _sub_gps.get().vel_n_m_s; + y(4) = _sub_gps.get().vel_e_m_s; + y(5) = _sub_gps.get().vel_d_m_s; + + // gps measurement matrix, measures position and velocity + Matrix C; + C.setZero(); + C(Y_gps_x, X_x) = 1; + C(Y_gps_y, X_y) = 1; + C(Y_gps_z, X_z) = 1; + C(Y_gps_vx, X_vx) = 1; + C(Y_gps_vy, X_vy) = 1; + C(Y_gps_vz, X_vz) = 1; + + // gps covariance matrix + Matrix R; + R.setZero(); + + // default to parameter, use gps cov if provided + float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get(); + float var_z = _gps_z_stddev.get() * _gps_z_stddev.get(); + float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get(); + float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get(); + + // if field is not zero, set it to the value provided + if (_sub_gps.get().eph > 1e-3f) { + var_xy = _sub_gps.get().eph * _sub_gps.get().eph; + } + + if (_sub_gps.get().epv > 1e-3f) { + var_z = _sub_gps.get().epv * _sub_gps.get().epv; + } + + // TODO is velocity covariance provided from gps sub + R(0, 0) = var_xy; + R(1, 1) = var_xy; + R(2, 2) = var_z; + R(3, 3) = var_vxy; + R(4, 4) = var_vxy; + R(5, 5) = var_vz; + + // residual + Vector r = y - C * _x; + Matrix S_I = inv(C * _P * C.transpose() + R); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + uint8_t nSat = _sub_gps.get().satellites_used; + float eph = _sub_gps.get().eph; + + if (nSat < 6 || eph > _gps_eph_max.get()) { + if (!_gpsFault) { + mavlink_log_info(_mavlink_fd, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); + warnx("[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); + _gpsFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_gpsFault) { + mavlink_log_info(_mavlink_fd, "[lpe] gps fault, beta: %5.2f", double(beta)); + warnx("[lpe] gps fault, beta: %5.2f", double(beta)); + mavlink_log_info(_mavlink_fd, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + double(r(0)), double(r(1)), double(r(2)), + double(r(3)), double(r(4)), double(r(5))); + mavlink_log_info(_mavlink_fd, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)), + double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5))); + mavlink_log_info(_mavlink_fd, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + double(r(0)), double(r(1)), double(r(2)), + double(r(3)), double(r(4)), double(r(5))); + _gpsFault = FAULT_MINOR; + } + + // trust GPS less + S_I = inv((C * _P * C.transpose()) + R * 10); + + } else if (_gpsFault) { + _gpsFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] GPS OK"); + warnx("[lpe] GPS OK"); + } + + // kalman filter correction if no hard fault + if (_gpsFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_gps = _timeStamp; +} + +void BlockLocalPositionEstimator::correctVision() +{ + + Vector y; + y.setZero(); + y(0) = _sub_vision_pos.get().x - _visionHome(0); + y(1) = _sub_vision_pos.get().y - _visionHome(1); + y(2) = _sub_vision_pos.get().z - _visionHome(2); + + // vision measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_vision_x, X_x) = 1; + C(Y_vision_y, X_y) = 1; + C(Y_vision_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); + R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); + R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get(); + + // residual + Matrix S_I = inv((C * _P * C.transpose()) + R); + Matrix r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_visionFault) { + mavlink_log_info(_mavlink_fd, "[lpe] vision position fault, beta %5.2f", double(beta)); + warnx("[lpe] vision position fault, beta %5.2f", double(beta)); + _visionFault = FAULT_MINOR; + } + + // trust less + S_I = inv((C * _P * C.transpose()) + R * 10); + + } else if (_visionFault) { + _visionFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] vision position OK"); + warnx("[lpe] vision position OK"); + } + + // kalman filter correction if no fault + if (_visionFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_vision_p = _sub_vision_pos.get().timestamp_boot; +} + +void BlockLocalPositionEstimator::correctmocap() +{ + + Vector y; + y.setZero(); + y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0); + y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1); + y(Y_mocap_z) = _sub_mocap.get().z - _mocapHome(2); + + // mocap measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_mocap_x, X_x) = 1; + C(Y_mocap_y, X_y) = 1; + C(Y_mocap_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + float mocap_p_var = _mocap_p_stddev.get()* \ + _mocap_p_stddev.get(); + R(Y_mocap_x, Y_mocap_x) = mocap_p_var; + R(Y_mocap_y, Y_mocap_y) = mocap_p_var; + R(Y_mocap_z, Y_mocap_z) = mocap_p_var; + + // residual + Matrix S_I = inv((C * _P * C.transpose()) + R); + Matrix r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_mocapFault) { + mavlink_log_info(_mavlink_fd, "[lpe] mocap fault, beta %5.2f", double(beta)); + warnx("[lpe] mocap fault, beta %5.2f", double(beta)); + _mocapFault = FAULT_MINOR; + } + + // trust less + S_I = inv((C * _P * C.transpose()) + R * 10); + + } else if (_mocapFault) { + _mocapFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] mocap OK"); + warnx("[lpe] mocap OK"); + } + + // kalman filter correction if no fault + if (_mocapFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_mocap = _sub_mocap.get().timestamp_boot; +} diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp new file mode 100644 index 000000000000..1c6c43fd201f --- /dev/null +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -0,0 +1,310 @@ +#pragma once + +#include +#include +#include +#include + +#ifdef USE_MATRIX_LIB +#include "matrix/Matrix.hpp" +using namespace matrix; +#else +#include +using namespace Eigen; +#endif + +// uORB Subscriptions +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// uORB Publications +#include +#include +#include +#include +#include + +#define CBRK_NO_VISION_KEY 328754 + +using namespace control; + +enum fault_t { + FAULT_NONE = 0, + FAULT_MINOR, + FAULT_SEVERE +}; + +enum sensor_t { + SENSOR_BARO = 0, + SENSOR_GPS, + SENSOR_LIDAR, + SENSOR_FLOW, + SENSOR_SONAR, + SENSOR_VISION, + SENSOR_MOCAP +}; + +class BlockLocalPositionEstimator : public control::SuperBlock +{ +// +// The purpose of this estimator is to provide a robust solution for +// indoor flight. +// +// dynamics: +// +// x(+) = A * x(-) + B * u(+) +// y_i = C_i*x +// +// kalman filter +// +// E[xx'] = P +// E[uu'] = W +// E[y_iy_i'] = R_i +// +// prediction +// x(+|-) = A*x(-|-) + B*u(+) +// P(+|-) = A*P(-|-)*A' + B*W*B' +// +// correction +// x(+|+) = x(+|-) + K_i * (y_i - H_i * x(+|-) ) +// +// +// input: +// ax, ay, az (acceleration NED) +// +// states: +// px, py, pz , ( position NED) +// vx, vy, vz ( vel NED), +// bx, by, bz ( TODO accelerometer bias) +// tz (TODO terrain altitude) +// +// measurements: +// +// sonar: pz (measured d*cos(phi)*cos(theta)) +// +// baro: pz +// +// flow: vx, vy (flow is in body x, y frame) +// +// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame) +// +// lidar: px (actual measured d*cos(phi)*cos(theta)) +// +// vision: px, py, pz, vx, vy, vz +// +// mocap: px, py, pz +// +public: + BlockLocalPositionEstimator(); + void update(); + virtual ~BlockLocalPositionEstimator(); + +private: + // prevent copy and assignment + BlockLocalPositionEstimator(const BlockLocalPositionEstimator &); + BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &); + + // constants + static const uint8_t n_x = 9; + static const uint8_t n_u = 3; // 3 accelerations + static const uint8_t n_y_flow = 2; + static const uint8_t n_y_sonar = 1; + static const uint8_t n_y_baro = 1; + static const uint8_t n_y_lidar = 1; + static const uint8_t n_y_gps = 6; + static const uint8_t n_y_vision = 3; + static const uint8_t n_y_mocap = 3; + enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz}; + enum {U_ax = 0, U_ay, U_az}; + enum {Y_baro_z = 0}; + enum {Y_lidar_z = 0}; + enum {Y_flow_x = 0, Y_flow_y}; + enum {Y_sonar_z = 0}; + enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz}; + enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, Y_vision_vx, Y_vision_vy, Y_vision_vz}; + enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z}; + enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM}; + + // methods + // ---------------------------- + void initP(); + + // predict the next state + void predict(); + + // correct the state prediction wtih a measurement + void correctBaro(); + void correctGps(); + void correctLidar(); + void correctFlow(); + void correctSonar(); + void correctVision(); + void correctmocap(); + + // sensor initialization + void updateHome(); + void initBaro(); + void initGps(); + void initLidar(); + void initSonar(); + void initFlow(); + void initVision(); + void initmocap(); + + // publications + void publishLocalPos(); + void publishGlobalPos(); + void publishFilteredFlow(); + void publishEstimatorStatus(); + + // attributes + // ---------------------------- + + // subscriptions + uORB::Subscription _sub_status; + uORB::Subscription _sub_armed; + uORB::Subscription _sub_control_mode; + uORB::Subscription _sub_att; + uORB::Subscription _sub_att_sp; + uORB::Subscription _sub_flow; + uORB::Subscription _sub_sensor; + uORB::Subscription _sub_distance; + uORB::Subscription _sub_param_update; + uORB::Subscription _sub_manual; + uORB::Subscription _sub_home; + uORB::Subscription _sub_gps; + uORB::Subscription _sub_vision_pos; + uORB::Subscription _sub_mocap; + + // publications + uORB::Publication _pub_lpos; + uORB::Publication _pub_gpos; + uORB::Publication _pub_filtered_flow; + uORB::Publication _pub_est_status; + + // map projection + struct map_projection_reference_s _map_ref; + + // parameters + BlockParamInt _integrate; + + BlockParamFloat _flow_xy_stddev; + BlockParamFloat _sonar_z_stddev; + + BlockParamFloat _lidar_z_stddev; + + BlockParamFloat _accel_xy_stddev; + BlockParamFloat _accel_z_stddev; + + BlockParamFloat _baro_stddev; + + BlockParamFloat _gps_xy_stddev; + BlockParamFloat _gps_z_stddev; + + BlockParamFloat _gps_vxy_stddev; + BlockParamFloat _gps_vz_stddev; + + BlockParamFloat _gps_eph_max; + + BlockParamFloat _vision_xy_stddev; + BlockParamFloat _vision_z_stddev; + BlockParamInt _no_vision; + BlockParamFloat _beta_max; + + BlockParamFloat _mocap_p_stddev; + + // process noise + BlockParamFloat _pn_p_noise_power; + BlockParamFloat _pn_v_noise_power; + BlockParamFloat _pn_b_noise_power; + + // misc + struct pollfd _polls[3]; + uint64_t _timeStamp; + uint64_t _time_last_xy; + uint64_t _time_last_flow; + uint64_t _time_last_baro; + uint64_t _time_last_gps; + uint64_t _time_last_lidar; + uint64_t _time_last_sonar; + uint64_t _time_last_vision_p; + uint64_t _time_last_mocap; + int _mavlink_fd; + + // initialization flags + bool _baroInitialized; + bool _gpsInitialized; + bool _lidarInitialized; + bool _sonarInitialized; + bool _flowInitialized; + bool _visionInitialized; + bool _mocapInitialized; + + // init counts + int _baroInitCount; + int _gpsInitCount; + int _lidarInitCount; + int _sonarInitCount; + int _flowInitCount; + int _visionInitCount; + int _mocapInitCount; + + // reference altitudes + float _altHome; + bool _altHomeInitialized; + float _baroAltHome; + float _gpsAltHome; + float _lidarAltHome; + float _sonarAltHome; + float _flowAltHome; + Vector3f _visionHome; + Vector3f _mocapHome; + + // flow integration + float _flowX; + float _flowY; + float _flowMeanQual; + + // referene lat/lon + double _gpsLatHome; + double _gpsLonHome; + + // status + bool _canEstimateXY; + bool _canEstimateZ; + bool _xyTimeout; + + // sensor faults + fault_t _baroFault; + fault_t _gpsFault; + fault_t _lidarFault; + fault_t _flowFault; + fault_t _sonarFault; + fault_t _visionFault; + fault_t _mocapFault; + + bool _visionTimeout; + bool _mocapTimeout; + + perf_counter_t _loop_perf; + perf_counter_t _interval_perf; + perf_counter_t _err_perf; + + // state space + Vector _x; // state vector + Vector _u; // input vector + Matrix _P; // state covariance matrix +}; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt new file mode 100644 index 000000000000..761a0d94b045 --- /dev/null +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -0,0 +1,55 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if(${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500) +elseif(${OS} STREQUAL "posix") + list(APPEND MODULE_CFLAGS -Wno-error) +endif() + +# use custom matrix lib instead of Eigen +add_definitions(-DUSE_MATRIX_LIB) + + +px4_add_module( + MODULE modules__local_position_estimator + MAIN local_position_estimator + STACK 9216 + COMPILE_FLAGS ${MODULE_CFLAGS} + SRCS + local_position_estimator_main.cpp + BlockLocalPositionEstimator.cpp + params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp new file mode 100644 index 000000000000..b0cd25554e21 --- /dev/null +++ b/src/modules/local_position_estimator/local_position_estimator_main.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file local_position_estimator.cpp + * @author James Goppert + * @author Mohammed Kabir + * @author Nuno Marques + * + * Local position estimator + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "BlockLocalPositionEstimator.hpp" + +static volatile bool thread_should_exit = false; /**< Deamon exit flag */ +static volatile bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int local_position_estimator_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static int usage(const char *reason); + +static int +usage(const char *reason) +{ + if (reason) { + fprintf(stderr, "%s\n", reason); + } + + fprintf(stderr, "usage: local_position_estimator {start|stop|status} [-p ]\n\n"); + return 1; +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int local_position_estimator_main(int argc, char *argv[]) +{ + + if (argc < 1) { + usage("missing command"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + return 0; + } + + thread_should_exit = false; + + deamon_task = px4_task_spawn_cmd("lp_estimator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 10240, + local_position_estimator_thread_main, + (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL); + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (thread_running) { + warnx("stop"); + thread_should_exit = true; + + } else { + warnx("not started"); + } + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + return 0; + } + + usage("unrecognized command"); + return 1; +} + +int local_position_estimator_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + using namespace control; + + + thread_running = true; + + BlockLocalPositionEstimator est; + + while (!thread_should_exit) { + est.update(); + } + + warnx("exiting."); + + thread_running = false; + + return 0; +} diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c new file mode 100644 index 000000000000..8fbde4ab690d --- /dev/null +++ b/src/modules/local_position_estimator/params.c @@ -0,0 +1,222 @@ +#include + +// 16 is max name length + + +/** + * Enable local position estimator. + * + * @group Local Position Estimator + */ +PARAM_DEFINE_INT32(LPE_ENABLED, 1); + +/** + * Enable accelerometer integration for prediction. + * + * @group Local Position Estimator + */ +PARAM_DEFINE_INT32(LPE_INTEGRATE, 1); + +/** + * Optical flow xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f); + +/** + * Sonar z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.2f); + +/** + * Lidar z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f); + +/** + * Accelerometer xy standard deviation + * + * Data sheet sqrt(Noise power) = 150ug/sqrt(Hz) + * std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2 + * Since accels sampled at 1000 Hz. + * + * should be 0.0464 + * + * @group Local Position Estimator + * @unit m/s^2 + * @min 0.00001 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f); + +/** + * Accelerometer z standard deviation + * + * (see Accel x comments) + * + * @group Local Position Estimator + * @unit m/s^2 + * @min 0.00001 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f); + +/** + * Barometric presssure altitude z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 3 + */ +PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f); + +/** + * GPS xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 5 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f); + +/** + * GPS z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 20 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f); + +/** + * GPS xy velocity standard deviation. + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f); + +/** + * GPS z velocity standard deviation. + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f); + +/** + * GPS max eph + * + * @group Local Position Estimator + * @unit m + * @min 1.0 + * @max 5.0 + */ +PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f); + + + +/** + * Vision xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f); + +/** + * Vision z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f); + +/** + * Circuit breaker to disable vision input. + * + * Set to the appropriate key (328754) to disable vision input. + * + * @group Local Position Estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(LPE_NO_VISION, 0); + +/** + * Vicon position standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f); + +/** + * Position propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s^2)-s + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f); + +/** + * Velocity propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s)-s + * @min 0 + * @max 5 + */ +PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f); + +/** + * Accel bias propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s)-s + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-8f); + +/** + * Fault detection threshold, for chi-squared dist. + * + * TODO add separate params for 1 dof, 3 dof, and 6 dof beta + * or false alarm rate in false alarms/hr + * + * @group Local Position Estimator + * @unit + * @min 3 + * @max 1000 + */ +PARAM_DEFINE_FLOAT(LPE_BETA_MAX, 1000.0f); diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt new file mode 100644 index 000000000000..c8a0e7d870c5 --- /dev/null +++ b/src/modules/mavlink/CMakeLists.txt @@ -0,0 +1,62 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if (${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=1500) +endif() +px4_add_module( + MODULE modules__mavlink + MAIN mavlink + STACK 1200 + COMPILE_FLAGS + ${MODULE_CFLAGS} + -Wno-attributes + -Wno-packed + -DMAVLINK_COMM_NUM_BUFFERS=3 + -Wno-packed + -Wno-tautological-constant-out-of-range-compare + -Os + SRCS + mavlink.c + mavlink_main.cpp + mavlink_mission.cpp + mavlink_parameters.cpp + mavlink_orb_subscription.cpp + mavlink_messages.cpp + mavlink_stream.cpp + mavlink_rate_limiter.cpp + mavlink_receiver.cpp + mavlink_ftp.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 857c83f39234..1073d09c20cd 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -33,9 +33,9 @@ /** * @file mavlink.c - * Adapter functions expected by the protocol library + * Define MAVLink specific parameters * - * @author Lorenz Meier + * @author Lorenz Meier */ #include @@ -46,60 +46,6 @@ #include "mavlink_bridge_header.h" #include -/* define MAVLink specific parameters */ -/** - * MAVLink system ID - * @group MAVLink - * @min 1 - * @max 250 - */ -PARAM_DEFINE_INT32(MAV_SYS_ID, 1); - -/** - * MAVLink component ID - * @group MAVLink - * @min 1 - * @max 50 - */ -PARAM_DEFINE_INT32(MAV_COMP_ID, 50); - -/** - * MAVLink type - * @group MAVLink - */ -PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); - -/** - * Use/Accept HIL GPS message (even if not in HIL mode) - * - * If set to 1 incomming HIL GPS messages are parsed. - * - * @group MAVLink - */ -PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); - -/** - * Forward external setpoint messages - * - * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard - * control mode - * - * @group MAVLink - */ -PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); - -/** - * Test parameter - * - * This parameter is not actively used by the system. Its purpose is to allow - * testing the parameter interface on the communication level. - * - * @group MAVLink - * @min -1000 - * @max 1000 - */ -PARAM_DEFINE_INT32(MAV_TEST_PAR, 1); - mavlink_system_t mavlink_system = { 100, 50 diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 36d2f2ff6a2a..c074d1dde643 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -299,7 +299,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req) /// @brief Responds to a List command MavlinkFTP::ErrorCode -MavlinkFTP::_workList(PayloadHeader* payload) +MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden) { char dirPath[kMaxDataLength]; strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); @@ -383,7 +383,8 @@ MavlinkFTP::_workList(PayloadHeader* payload) #else case DT_DIR: #endif - if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + if ((!list_hidden && (strncmp(entry.d_name, ".", 1) == 0)) || + strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { // Don't bother sending these back direntType = kDirentSkip; } else { @@ -454,7 +455,8 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) } fileSize = st.st_size; - int fd = ::open(filename, oflag); + // Set mode to 666 incase oflag has O_CREAT + int fd = ::open(filename, oflag, PX4_O_MODE_666); if (fd < 0) { return kErrFailErrno; } @@ -571,7 +573,7 @@ MavlinkFTP::ErrorCode MavlinkFTP::_workTruncateFile(PayloadHeader* payload) { char file[kMaxDataLength]; - const char temp_file[] = "/fs/microsd/.trunc.tmp"; + const char temp_file[] = PX4_ROOTFSDIR"/fs/microsd/.trunc.tmp"; strncpy(file, _data_as_cstring(payload), kMaxDataLength); payload->size = 0; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 33b8996f712d..998c6b3cc6b6 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -130,7 +130,7 @@ class MavlinkFTP : public MavlinkStream void _reply(mavlink_file_transfer_protocol_t* ftp_req); int _copy_file(const char *src_path, const char *dst_path, size_t length); - ErrorCode _workList(PayloadHeader *payload); + ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false); ErrorCode _workOpen(PayloadHeader *payload, int oflag); ErrorCode _workRead(PayloadHeader *payload); ErrorCode _workBurst(PayloadHeader* payload, uint8_t target_system_id); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7c739cfc346e..1b04eea1a028 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -94,8 +94,9 @@ #endif static const int ERROR = -1; +#define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 1000000 ///< max data rate in bytes/s +#define MAX_DATA_RATE 10000000 ///< max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate #define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it. @@ -120,6 +121,8 @@ extern mavlink_system_t mavlink_system; static void usage(void); +bool Mavlink::_boot_complete = false; + Mavlink::Mavlink() : #ifndef __PX4_NUTTX VDev("mavlink-log", MAVLINK_LOG_DEVICE), @@ -145,12 +148,12 @@ Mavlink::Mavlink() : _mavlink_ftp(nullptr), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), + _radio_id(0), _logbuffer {}, _total_counter(0), _receive_thread {}, _verbose(false), _forwarding_on(false), - _passing_on(false), _ftp_on(false), #ifndef __PX4_POSIX _uart_fd(-1), @@ -159,10 +162,12 @@ Mavlink::Mavlink() : _datarate(1000), _datarate_events(500), _rate_mult(1.0f), + _last_hw_rate_timestamp(0), _mavlink_param_queue_index(0), mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), + _udp_initialised(false), _flow_control_enabled(true), _last_write_success_time(0), _last_write_try_time(0), @@ -173,6 +178,14 @@ Mavlink::Mavlink() : _rate_tx(0.0f), _rate_txerr(0.0f), _rate_rx(0.0f), +#ifdef __PX4_POSIX + _myaddr{}, + _src_addr{}, + _bcast_addr{}, +#endif + _socket_fd(-1), + _protocol(SERIAL), + _network_port(14556), _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, @@ -180,6 +193,7 @@ Mavlink::Mavlink() : _param_initialized(false), _param_system_id(0), _param_component_id(0), + _param_radio_id(0), _param_system_type(MAV_TYPE_FIXED_WING), _param_use_hil_gps(0), _param_forward_externalsp(0), @@ -237,7 +251,7 @@ Mavlink::Mavlink() : break; } - _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + _rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC; } Mavlink::~Mavlink() @@ -325,6 +339,20 @@ Mavlink::get_instance_for_device(const char *device_name) return nullptr; } +Mavlink * +Mavlink::get_instance_for_network_port(unsigned long port) +{ + Mavlink *inst; + + LL_FOREACH(::_mavlink_instances, inst) { + if (inst->_network_port == port) { + return inst; + } + } + + return nullptr; +} + int Mavlink::destroy_all_instances() { @@ -417,7 +445,6 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) } } -#ifndef __PX4_POSIX int Mavlink::get_uart_fd(unsigned index) { @@ -435,7 +462,6 @@ Mavlink::get_uart_fd() { return _uart_fd; } -#endif // __PX4_POSIX int Mavlink::get_instance_id() @@ -508,6 +534,7 @@ void Mavlink::mavlink_update_system(void) if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); + _param_radio_id = param_find("MAV_RADIO_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); _param_forward_externalsp = param_find("MAV_FWDEXTSP"); @@ -523,6 +550,7 @@ void Mavlink::mavlink_update_system(void) int32_t component_id; param_get(_param_component_id, &component_id); + param_get(_param_radio_id, &_radio_id); /* only allow system ID and component ID updates * after reboot - not during operation */ @@ -670,8 +698,16 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } else { _is_usb_uart = true; + /* USB has no baudrate, but use a magic number for 'fast' */ + _baudrate = 2000000; + _rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB; } +#if defined (__PX4_LINUX) || defined (__PX4_DARWIN) + /* Put in raw mode */ + cfmakeraw(&uart_config); +#endif + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { warnx("ERR SET CONF %s\n", uart_name); ::close(_uart_fd); @@ -768,7 +804,7 @@ Mavlink::get_free_tx_buf() #ifndef __PX4_POSIX // No FIONWRITE on Linux -#if !defined(__PX4_LINUX) +#if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN) (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); #endif @@ -784,8 +820,14 @@ Mavlink::get_free_tx_buf() enable_flow_control(false); } } + #endif + // if we are using network sockets, return max lenght of one packet + if (get_protocol() == UDP || get_protocol() == TCP ) { + return 1500; + } + return buf_free; } @@ -800,20 +842,21 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID pthread_mutex_lock(&_send_mutex); - unsigned buf_free = get_free_tx_buf(); - uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; _last_write_try_time = hrt_absolute_time(); - /* check if there is space in the buffer, let it overflow else */ - if (buf_free < packet_len) { - /* no enough space in buffer to send */ - count_txerr(); - count_txerrbytes(packet_len); - pthread_mutex_unlock(&_send_mutex); - return; + if (get_protocol() == SERIAL) { + /* check if there is space in the buffer, let it overflow else */ + unsigned buf_free = get_free_tx_buf(); + if (buf_free < packet_len) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; + } } uint8_t buf[MAVLINK_MAX_PACKET_LEN]; @@ -839,11 +882,37 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + size_t ret = -1; #ifndef __PX4_POSIX /* send message to UART */ - ssize_t ret = ::write(_uart_fd, buf, packet_len); + if (get_protocol() == SERIAL) { + ret = ::write(_uart_fd, buf, packet_len); + } +#else + if (get_protocol() == UDP) { + ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); - if (ret != (int) packet_len) { + struct telemetry_status_s &tstatus = get_rx_status(); + + /* resend heartbeat via broadcast */ + if (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || + (tstatus.heartbeat_time == 0)) && + msgid == MAVLINK_MSG_ID_HEARTBEAT) { + + int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr)); + + if (bret <= 0) { + PX4_WARN("sending broadcast failed"); + } + } + + } else if (get_protocol() == TCP) { + /* not implemented, but possible to do so */ + warnx("TCP transport pending implementation"); + } +#endif + + if (ret != (size_t) packet_len) { count_txerr(); count_txerrbytes(packet_len); @@ -851,7 +920,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } -#endif pthread_mutex_unlock(&_send_mutex); } @@ -903,11 +971,54 @@ Mavlink::resend_message(mavlink_message_t *msg) _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } + #endif pthread_mutex_unlock(&_send_mutex); } +void +Mavlink::init_udp() +{ +#if defined (__PX4_LINUX) || defined (__PX4_DARWIN) + PX4_INFO("Setting up UDP w/port %d",_network_port); + + memset((char *)&_myaddr, 0, sizeof(_myaddr)); + _myaddr.sin_family = AF_INET; + _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + _myaddr.sin_port = htons(_network_port); + + if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + PX4_WARN("create socket failed"); + return; + } + + int broadcast_opt = 1; + if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) { + PX4_WARN("setting broadcast permission failed"); + return; + } + + if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { + PX4_WARN("bind failed"); + return; + } + + /* set default target address */ + memset((char *)&_src_addr, 0, sizeof(_src_addr)); + _src_addr.sin_family = AF_INET; + inet_aton("127.0.0.1", &_src_addr.sin_addr); + _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + + /* default broadcast address */ + memset((char *)&_bcast_addr, 0, sizeof(_bcast_addr)); + _bcast_addr.sin_family = AF_INET; + inet_aton("255.255.255.255", &_bcast_addr.sin_addr); + _bcast_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + +#endif +} + void Mavlink::handle_message(const mavlink_message_t *msg) { @@ -954,7 +1065,8 @@ Mavlink::send_statustext(unsigned char severity, const char *string) mavlink_logbuffer_write(&_logbuffer, &logmsg); } -void Mavlink::send_autopilot_capabilites() { +void Mavlink::send_autopilot_capabilites() +{ struct vehicle_status_s status; MavlinkOrbSubscription *status_sub = this->add_orb_subscription(ORB_ID(vehicle_status)); @@ -977,16 +1089,16 @@ void Mavlink::send_autopilot_capabilites() { memcpy(&msg.flight_custom_version, &px4_git_version_binary, sizeof(msg.flight_custom_version)); memcpy(&msg.middleware_custom_version, &px4_git_version_binary, sizeof(msg.middleware_custom_version)); memset(&msg.os_custom_version, 0, sizeof(msg.os_custom_version)); - #ifdef CONFIG_CDCACM_VENDORID +#ifdef CONFIG_CDCACM_VENDORID msg.vendor_id = CONFIG_CDCACM_VENDORID; - #else +#else msg.vendor_id = 0; - #endif - #ifdef CONFIG_CDCACM_PRODUCTID +#endif +#ifdef CONFIG_CDCACM_PRODUCTID msg.product_id = CONFIG_CDCACM_PRODUCTID; - #else +#else msg.product_id = 0; - #endif +#endif uint32_t uid[3]; mcu_unique_id(uid); msg.uid = (((uint64_t)uid[1]) << 32) | uid[2]; @@ -1244,7 +1356,7 @@ Mavlink::message_buffer_mark_read(int n) void Mavlink::pass_message(const mavlink_message_t *msg) { - if (_passing_on) { + if (_forwarding_on) { /* size is 8 bytes plus variable payload */ int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; pthread_mutex_lock(&_message_buffer_mutex); @@ -1265,6 +1377,7 @@ Mavlink::update_rate_mult() float const_rate = 0.0f; float rate = 0.0f; + /* scale down rates if their theoretical bandwidth is exceeding the link bandwidth */ MavlinkStream *stream; LL_FOREACH(_streams, stream) { if (stream->const_rate()) { @@ -1276,7 +1389,55 @@ Mavlink::update_rate_mult() } /* don't scale up rates, only scale down if needed */ - _rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); + float bandwidth_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); + + /* check if we have radio feedback */ + struct telemetry_status_s &tstatus = get_rx_status(); + + bool radio_critical = false; + bool radio_found = false; + + /* 2nd pass: Now check hardware limits */ + if (tstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) { + + radio_found = true; + + if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) { + radio_critical = true; + } + } + + float hardware_mult = _rate_mult; + + /* scale down if we have a TX err rate suggesting link congestion */ + if (_rate_txerr > 0.0f && !radio_critical) { + hardware_mult = (_rate_tx) / (_rate_tx + _rate_txerr); + } else if (radio_found && tstatus.timestamp != _last_hw_rate_timestamp) { + + if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) { + /* this indicates link congestion, reduce rate by 20% */ + hardware_mult *= 0.80f; + } else if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) { + /* this indicates link congestion, reduce rate by 2.5% */ + hardware_mult *= 0.975f; + } else if (tstatus.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) { + /* this indicates spare bandwidth, increase by 2.5% */ + hardware_mult *= 1.025f; + /* limit to a max multiplier of 1 */ + hardware_mult = fminf(1.0f, hardware_mult); + } + } else { + /* no limitation, set hardware to 1 */ + hardware_mult = 1.0f; + } + + _last_hw_rate_timestamp = tstatus.timestamp; + + /* pick the minimum from bandwidth mult and hardware mult as limit */ + _rate_mult = fminf(bandwidth_mult, hardware_mult); + + /* ensure the rate multiplier never drops below 5% so that something is always sent */ + _rate_mult = fmaxf(0.05f, _rate_mult); } int @@ -1288,7 +1449,12 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_NORMAL; #ifdef __PX4_NUTTX - /* work around some stupidity in task_create's argv handling */ + /* the NuttX optarg handler does not + * ignore argv[0] like the POSIX handler + * does, nor does it deal with non-flag + * verbs well. Remove the application + * name and the verb. + */ argc -= 2; argv += 2; #endif @@ -1296,10 +1462,12 @@ Mavlink::task_main(int argc, char *argv[]) /* don't exit from getopt loop to leave getopt global variables in consistent state, * set error flag instead */ bool err_flag = false; - int myoptind=1; + int myoptind = 1; const char *myoptarg = NULL; + char* eptr; + int temp_int_arg; - while ((ch = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:u:m:fpvwx", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(myoptarg, NULL, 10); @@ -1323,6 +1491,18 @@ Mavlink::task_main(int argc, char *argv[]) case 'd': _device_name = myoptarg; + set_protocol(SERIAL); + break; + + case 'u': + temp_int_arg = strtoul(myoptarg, &eptr, 10); + if ( *eptr == '\0' ) { + _network_port = temp_int_arg; + set_protocol(UDP); + } else { + warnx("invalid data udp_port '%s'", myoptarg); + err_flag = true; + } break; // case 'e': @@ -1336,8 +1516,18 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "camera") == 0) { // left in here for compatibility _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(myoptarg, "onboard") == 0) { _mode = MAVLINK_MODE_ONBOARD; + + } else if (strcmp(myoptarg, "osd") == 0) { + _mode = MAVLINK_MODE_OSD; + + } else if (strcmp(myoptarg, "magic") == 0) { + _mode = MAVLINK_MODE_MAGIC; + + } else if (strcmp(myoptarg, "config") == 0) { + _mode = MAVLINK_MODE_CONFIG; } break; @@ -1346,10 +1536,6 @@ Mavlink::task_main(int argc, char *argv[]) _forwarding_on = true; break; - case 'p': - _passing_on = true; - break; - case 'v': _verbose = true; break; @@ -1387,8 +1573,11 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + if (get_protocol() == SERIAL) { warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); - + } else if (get_protocol() == UDP) { + warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port); + } /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1402,6 +1591,7 @@ Mavlink::task_main(int argc, char *argv[]) warn("could not open %s", _device_name); return ERROR; } + #endif /* initialize send mutex */ @@ -1411,7 +1601,7 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_init(&_logbuffer, 5); /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ - if (_passing_on || _ftp_on) { + if (_forwarding_on || _ftp_on) { /* initialize message buffer if multiplexing is on or its needed for FTP. * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. @@ -1429,7 +1619,11 @@ Mavlink::task_main(int argc, char *argv[]) #ifdef __PX4_NUTTX register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); #else - register_driver(MAVLINK_LOG_DEVICE, NULL); + int ret; + ret = VDev::init(); + if (ret != OK) { + PX4_WARN("VDev setup for mavlink log device failed!\n"); + } #endif /* initialize logging device */ @@ -1441,8 +1635,6 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - _task_running = true; - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); uint64_t param_time = 0; MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); @@ -1483,41 +1675,92 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); - configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 10.0f); + configure_stream("HOME_POSITION", 0.5f); + configure_stream("HIGHRES_IMU", 2.0f); + configure_stream("ATTITUDE", 20.0f); configure_stream("VFR_HUD", 8.0f); configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); - configure_stream("RC_CHANNELS_RAW", 1.0f); + configure_stream("RC_CHANNELS", 1.0f); + configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); - configure_stream("ATTITUDE_TARGET", 3.0f); + configure_stream("ATTITUDE_TARGET", 8.0f); configure_stream("DISTANCE_SENSOR", 0.5f); configure_stream("OPTICAL_FLOW_RAD", 5.0f); + configure_stream("EXTENDED_SYS_STATE", 1.0f); break; case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 50.0f); + configure_stream("ATTITUDE", 250.0f); configure_stream("HIGHRES_IMU", 50.0f); - configure_stream("VFR_HUD", 5.0f); configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); + configure_stream("NAMED_VALUE_FLOAT", 10.0f); configure_stream("CAMERA_CAPTURE", 2.0f); + configure_stream("HOME_POSITION", 0.5f); configure_stream("ATTITUDE_TARGET", 10.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); - configure_stream("RC_CHANNELS_RAW", 20.0f); + configure_stream("RC_CHANNELS", 20.0f); + configure_stream("SERVO_OUTPUT_RAW_0", 10.0f); configure_stream("VFR_HUD", 10.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); + //camera trigger is rate limited at the source, do not limit here + configure_stream("CAMERA_TRIGGER", 500.0f); + configure_stream("EXTENDED_SYS_STATE", 2.0f); break; + case MAVLINK_MODE_OSD: + configure_stream("SYS_STATUS", 5.0f); + configure_stream("ATTITUDE", 25.0f); + configure_stream("VFR_HUD", 5.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 10.0f); + configure_stream("HOME_POSITION", 0.5f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("BATTERY_STATUS", 1.0f); + configure_stream("SYSTEM_TIME", 1.0f); + configure_stream("RC_CHANNELS", 5.0f); + configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); + configure_stream("EXTENDED_SYS_STATE", 1.0f); + break; + + case MAVLINK_MODE_MAGIC: + //stream nothing + break; + + case MAVLINK_MODE_CONFIG: + // Enable a number of interesting streams we want via USB + configure_stream("SYS_STATUS", 1.0f); + configure_stream("HOME_POSITION", 0.5f); + configure_stream("GLOBAL_POSITION_INT", 10.0f); + configure_stream("ATTITUDE_TARGET", 8.0f); + configure_stream("PARAM_VALUE", 300.0f); + configure_stream("MISSION_ITEM", 50.0f); + configure_stream("NAMED_VALUE_FLOAT", 50.0f); + configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("DISTANCE_SENSOR", 10.0f); + configure_stream("VFR_HUD", 20.0f); + configure_stream("ATTITUDE", 100.0f); + configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); + configure_stream("RC_CHANNELS", 5.0f); + configure_stream("SERVO_OUTPUT_RAW_0", 20.0f); + configure_stream("SERVO_OUTPUT_RAW_1", 20.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("LOCAL_POSITION_NED", 30.0f); + configure_stream("MANUAL_CONTROL", 5.0f); + configure_stream("HIGHRES_IMU", 50.0f); + configure_stream("GPS_RAW_INT", 20.0f); + configure_stream("CAMERA_TRIGGER", 500.0f); + configure_stream("EXTENDED_SYS_STATE", 2.0f); + default: break; } @@ -1528,7 +1771,15 @@ Mavlink::task_main(int argc, char *argv[]) /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); - send_autopilot_capabilites(); + /* init socket if necessary */ + if (get_protocol() == UDP) { + init_udp(); + } + + /* if the protocol is serial, we send the system version blindly */ + if (get_protocol() == SERIAL) { + send_autopilot_capabilites(); + } while (!_task_should_exit) { /* main loop */ @@ -1539,12 +1790,58 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); update_rate_mult(); + + _mission_manager->check_active_mission(); if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ mavlink_update_system(); } + /* radio config check */ + if (_radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) { + /* request to configure radio and radio is present */ + FILE *fs = fdopen(_uart_fd, "w"); + + if (fs) { + /* switch to AT command mode */ + usleep(1200000); + fprintf(fs, "+++\n"); + usleep(1200000); + + if (_radio_id > 0) { + /* set channel */ + fprintf(fs, "ATS3=%u\n", _radio_id); + usleep(200000); + } else { + /* reset to factory defaults */ + fprintf(fs, "AT&F\n"); + usleep(200000); + } + + /* write config */ + fprintf(fs, "AT&W"); + usleep(200000); + + /* reboot */ + fprintf(fs, "ATZ"); + usleep(200000); + + // XXX NuttX suffers from a bug where + // fclose() also closes the fd, not just + // the file stream. Since this is a one-time + // config thing, we leave the file struct + // allocated. + //fclose(fs); + } else { + warnx("open fd %d failed", _uart_fd); + } + + /* reset param and save */ + _radio_id = 0; + param_set(_param_radio_id, &_radio_id); + } + if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); @@ -1556,15 +1853,28 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, - (double)_subscribe_to_stream_rate); + if ( get_protocol() == SERIAL ) { + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, + (double)_subscribe_to_stream_rate); + } else if ( get_protocol() == UDP ) { + warnx("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port, + (double)_subscribe_to_stream_rate); + } } else { - warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); + if ( get_protocol() == SERIAL ) { + warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); + } else if ( get_protocol() == UDP ) { + warnx("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port); + } } } else { - warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); + if ( get_protocol() == SERIAL ) { + warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); + } else if ( get_protocol() == UDP ) { + warnx("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port); + } } _subscribe_to_stream = nullptr; @@ -1577,7 +1887,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* pass messages from other UARTs or FTP worker */ - if (_passing_on || _ftp_on) { + if (_forwarding_on || _ftp_on) { bool is_part; uint8_t *read_ptr; @@ -1640,6 +1950,9 @@ Mavlink::task_main(int argc, char *argv[]) } perf_end(_loop_perf); + + /* confirm task running only once fully initialized */ + _task_running = true; } delete _subscribe_to_stream; @@ -1683,7 +1996,7 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ px4_close(_mavlink_fd); - if (_passing_on || _ftp_on) { + if (_forwarding_on || _ftp_on) { message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } @@ -1728,6 +2041,12 @@ Mavlink::start(int argc, char *argv[]) // before returning to the shell int ic = Mavlink::instance_count(); + if (ic == MAVLINK_COMM_NUM_BUFFERS) { + warnx("Maximum MAVLink instance count of %d reached.", + (int)MAVLINK_COMM_NUM_BUFFERS); + return 1; + } + // Instantiate thread char buf[24]; sprintf(buf, "mavlink_if%d", ic); @@ -1737,11 +2056,11 @@ Mavlink::start(int argc, char *argv[]) // task - start_helper() only returns // when the started task exits. px4_task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2400, - (px4_main_t)&Mavlink::start_helper, - (char * const *)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2400, + (px4_main_t)&Mavlink::start_helper, + (char *const *)argv); // Ensure that this shell command // does not return before the instance @@ -1781,12 +2100,16 @@ Mavlink::display_status() printf("\ttype:\t\t"); switch (_rstatus.type) { - case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: printf("3DR RADIO\n"); break; + case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB: + printf("USB CDC\n"); + break; + default: - printf("UNKNOWN RADIO\n"); + printf("GENERIC LINK OR RADIO\n"); break; } @@ -1815,7 +2138,18 @@ Mavlink::stream_command(int argc, char *argv[]) const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; const char *stream_name = nullptr; + unsigned short network_port = 0; + char* eptr; + int temp_int_arg; + bool provided_device = false; + bool provided_network_port = false; + /* + * Called via main with original argv + * mavlink start + * + * Remove 2 + */ argc -= 2; argv += 2; @@ -1837,13 +2171,22 @@ Mavlink::stream_command(int argc, char *argv[]) i++; } else if (0 == strcmp(argv[i], "-d") && i < argc - 1) { + provided_device = true; device_name = argv[i + 1]; i++; } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { stream_name = argv[i + 1]; i++; - + } else if (0 == strcmp(argv[i], "-u") && i < argc - 1) { + provided_network_port = true; + temp_int_arg = strtoul(argv[i + 1], &eptr, 10); + if ( *eptr == '\0' ) { + network_port = temp_int_arg; + } else { + err_flag = true; + } + i++; } else { err_flag = true; } @@ -1852,7 +2195,16 @@ Mavlink::stream_command(int argc, char *argv[]) } if (!err_flag && rate >= 0.0f && stream_name != nullptr) { - Mavlink *inst = get_instance_for_device(device_name); + + Mavlink *inst = nullptr; + if (provided_device && !provided_network_port) { + inst = get_instance_for_device(device_name); + } else if (provided_network_port && !provided_device) { + inst = get_instance_for_network_port(network_port); + } else if (provided_device && provided_network_port) { + warnx("please provide either a device name or a network port"); + return 1; + } if (inst != nullptr) { inst->configure_stream_threadsafe(stream_name, rate); @@ -1861,12 +2213,17 @@ Mavlink::stream_command(int argc, char *argv[]) // If the link is not running we should complain, but not fall over // because this is so easy to get wrong and not fatal. Warning is sufficient. - warnx("mavlink for device %s is not running", device_name); - return 0; + if (provided_device) { + warnx("mavlink for device %s is not running", device_name); + } else { + warnx("mavlink for network on port %hu is not running", network_port); + } + + return 1; } } else { - warnx("usage: mavlink stream [-d device] -s stream -r rate"); + warnx("usage: mavlink stream [-d device] [-u network_port] -s stream -r rate"); return 1; } @@ -1875,7 +2232,7 @@ Mavlink::stream_command(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-u udp_port] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); } int mavlink_main(int argc, char *argv[]) @@ -1902,6 +2259,10 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream_command(argc, argv); + } else if (!strcmp(argv[1], "boot_complete")) { + Mavlink::set_boot_complete(); + return 0; + } else { usage(); return 1; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index a658ca20ca86..37c66b596331 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -45,6 +45,9 @@ #ifdef __PX4_NUTTX #include #else +#include +#include +#include #include #endif #include @@ -65,6 +68,12 @@ #include "mavlink_parameters.h" #include "mavlink_ftp.h" +enum Protocol { + SERIAL = 0, + UDP, + TCP, +}; + #ifdef __PX4_NUTTX class Mavlink #else @@ -103,7 +112,9 @@ class Mavlink : public device::VDev static Mavlink *get_instance(unsigned instance); - static Mavlink *get_instance_for_device(const char *device_name); + static Mavlink *get_instance_for_device(const char *device_name); + + static Mavlink *get_instance_for_network_port(unsigned long port); static int destroy_all_instances(); @@ -113,11 +124,9 @@ class Mavlink : public device::VDev static void forward_message(const mavlink_message_t *msg, Mavlink *self); -#ifndef __PX4_QURT static int get_uart_fd(unsigned index); int get_uart_fd(); -#endif /** * Get the MAVLink system id. @@ -138,7 +147,10 @@ class Mavlink : public device::VDev enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_ONBOARD + MAVLINK_MODE_ONBOARD, + MAVLINK_MODE_OSD, + MAVLINK_MODE_MAGIC, + MAVLINK_MODE_CONFIG }; void set_mode(enum MAVLINK_MODE); @@ -154,6 +166,14 @@ class Mavlink : public device::VDev bool get_forwarding_on() { return _forwarding_on; } + /** + * Set the boot complete flag on all instances + * + * Setting the flag unblocks parameter transmissions, which are gated + * beforehand to ensure that the system is fully initialized. + */ + static void set_boot_complete() { _boot_complete = true; } + /** * Get the free space in the transmit buffer * @@ -190,6 +210,11 @@ class Mavlink : public device::VDev */ void set_manual_input_mode_generation(bool generation_enabled) { _generate_rc = generation_enabled; } + /** + * Set communication protocol for this mavlink instance + */ + void set_protocol(Protocol p) {_protocol = p;}; + /** * Get the manual input generation mode * @@ -264,6 +289,8 @@ class Mavlink : public device::VDev float get_rate_mult(); + float get_baudrate() { return _baudrate; } + /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } bool get_has_received_messages() { return _received_messages; } @@ -305,6 +332,16 @@ class Mavlink : public device::VDev unsigned get_system_type() { return _system_type; } + Protocol get_protocol() { return _protocol; }; + + unsigned short get_network_port() { return _network_port; } + + int get_socket_fd () { return _socket_fd; }; +#ifdef __PX4_POSIX + struct sockaddr_in * get_client_source_address() {return &_src_addr;}; +#endif + static bool boot_complete() { return _boot_complete; } + protected: Mavlink *next; @@ -313,6 +350,7 @@ class Mavlink : public device::VDev int _mavlink_fd; bool _task_running; + static bool _boot_complete; /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ @@ -320,8 +358,8 @@ class Mavlink : public device::VDev bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ @@ -335,6 +373,7 @@ class Mavlink : public device::VDev MAVLINK_MODE _mode; mavlink_channel_t _channel; + int32_t _radio_id; struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; @@ -343,7 +382,6 @@ class Mavlink : public device::VDev bool _verbose; bool _forwarding_on; - bool _passing_on; bool _ftp_on; #ifndef __PX4_QURT int _uart_fd; @@ -352,6 +390,7 @@ class Mavlink : public device::VDev int _datarate; ///< data rate for normal streams (attitude, position, etc.) int _datarate_events; ///< data rate for params, waypoints, text messages float _rate_mult; + hrt_abstime _last_hw_rate_timestamp; /** * If the queue index is not at 0, the queue sending @@ -364,6 +403,7 @@ class Mavlink : public device::VDev char *_subscribe_to_stream; float _subscribe_to_stream_rate; + bool _udp_initialised; bool _flow_control_enabled; uint64_t _last_write_success_time; @@ -377,6 +417,16 @@ class Mavlink : public device::VDev float _rate_txerr; float _rate_rx; +#ifdef __PX4_POSIX + struct sockaddr_in _myaddr; + struct sockaddr_in _src_addr; + struct sockaddr_in _bcast_addr; + +#endif + int _socket_fd; + Protocol _protocol; + unsigned short _network_port; + struct telemetry_status_s _rstatus; ///< receive status struct mavlink_message_buffer { @@ -394,6 +444,7 @@ class Mavlink : public device::VDev bool _param_initialized; param_t _param_system_id; param_t _param_component_id; + param_t _param_radio_id; param_t _param_system_type; param_t _param_use_hil_gps; param_t _param_forward_externalsp; @@ -411,6 +462,10 @@ class Mavlink : public device::VDev static unsigned int interval_from_rate(float rate); + static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25; + static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35; + static constexpr unsigned RADIO_BUFFER_HALF_PERCENTAGE = 50; + int configure_stream(const char *stream_name, const float rate); /** @@ -439,6 +494,8 @@ class Mavlink : public device::VDev */ void update_rate_mult(); + void init_udp(); + #ifdef __PX4_NUTTX static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); #else diff --git a/src/modules/mavlink/mavlink_main_posix.cpp b/src/modules/mavlink/mavlink_main_posix.cpp deleted file mode 100644 index 9ac7707cfda0..000000000000 --- a/src/modules/mavlink/mavlink_main_posix.cpp +++ /dev/null @@ -1,1818 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * 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 - * 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 mavlink_main.cpp - * MAVLink 1.0 protocol implementation. - * - * @author Lorenz Meier - * @author Julian Oes - * @author Anton Babushkin - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#ifndef __PX4_QURT -#include -#endif -#include -#include /* isinf / isnan checks */ - -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "mavlink_bridge_header.h" -#include "mavlink_main.h" -#include "mavlink_messages.h" -#include "mavlink_receiver.h" -#include "mavlink_rate_limiter.h" - -#ifndef MAVLINK_CRC_EXTRA -#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems -#endif - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -#define DEFAULT_DEVICE_NAME "/tmp/ttyS1" -#define MAX_DATA_RATE 20000 ///< max data rate in bytes/s -#define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate -#define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it. - -static Mavlink *_mavlink_instances = nullptr; - -static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; -static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; - -/** - * mavlink app start / stop handling function - * - * @ingroup apps - */ -extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); - -extern mavlink_system_t mavlink_system; - -static void usage(void); - -Mavlink::Mavlink() : - VDev("mavlink-log", MAVLINK_LOG_DEVICE), - _device_name(DEFAULT_DEVICE_NAME), - _task_should_exit(false), - next(nullptr), - _instance_id(0), - _mavlink_fd(-1), - _task_running(false), - _hil_enabled(false), - _use_hil_gps(false), - _forward_externalsp(false), - _is_usb_uart(false), - _wait_to_transmit(false), - _received_messages(false), - _main_loop_delay(1000), - _subscriptions(nullptr), - _streams(nullptr), - _mission_manager(nullptr), - _parameters_manager(nullptr), - _mode(MAVLINK_MODE_NORMAL), - _channel(MAVLINK_COMM_0), - _logbuffer {}, - _total_counter(0), - _receive_thread {}, - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), -#ifndef __PX4_QURT - _uart_fd(-1), -#endif - _baudrate(57600), - _datarate(1000), - _datarate_events(500), - _rate_mult(1.0f), - _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _last_write_success_time(0), - _last_write_try_time(0), - _bytes_tx(0), - _bytes_txerr(0), - _bytes_rx(0), - _bytes_timestamp(0), - _rate_tx(0.0f), - _rate_txerr(0.0f), - _rate_rx(0.0f), - _rstatus {}, - _message_buffer {}, - _message_buffer_mutex {}, - _send_mutex {}, - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(MAV_TYPE_FIXED_WING), - _param_use_hil_gps(0), - _param_forward_externalsp(0), - _system_type(0), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) -{ - _instance_id = Mavlink::instance_count(); - - /* set channel according to instance id */ - switch (_instance_id) { - case 0: - _channel = MAVLINK_COMM_0; - break; - - case 1: - _channel = MAVLINK_COMM_1; - break; - - case 2: - _channel = MAVLINK_COMM_2; - break; - - case 3: - _channel = MAVLINK_COMM_3; - break; -#ifdef MAVLINK_COMM_4 - - case 4: - _channel = MAVLINK_COMM_4; - break; -#endif -#ifdef MAVLINK_COMM_5 - - case 5: - _channel = MAVLINK_COMM_5; - break; -#endif -#ifdef MAVLINK_COMM_6 - - case 6: - _channel = MAVLINK_COMM_6; - break; -#endif - - default: - warnx("instance ID is out of range"); - px4_task_exit(1); - break; - } - - _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; -} - -Mavlink::~Mavlink() -{ - perf_free(_loop_perf); - perf_free(_txerr_perf); - - if (_task_running) { - /* task wakes up every 10ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - //TODO store main task handle in Mavlink instance to allow killing task - //task_delete(_mavlink_task); - break; - } - } while (_task_running); - } - - if (_mavlink_instances) - LL_DELETE(_mavlink_instances, this); -} - -void -Mavlink::count_txerr() -{ - perf_count(_txerr_perf); -} - -void -Mavlink::set_mode(enum MAVLINK_MODE mode) -{ - _mode = mode; -} - -int -Mavlink::instance_count() -{ - unsigned inst_index = 0; - Mavlink *inst; - - LL_FOREACH(::_mavlink_instances, inst) { - inst_index++; - } - - return inst_index; -} - -Mavlink * -Mavlink::get_instance(unsigned instance) -{ - Mavlink *inst; - unsigned inst_index = 0; - LL_FOREACH(::_mavlink_instances, inst) { - if (instance == inst_index) { - return inst; - } - - inst_index++; - } - - return nullptr; -} - -Mavlink * -Mavlink::get_instance_for_device(const char *device_name) -{ - Mavlink *inst; - - LL_FOREACH(::_mavlink_instances, inst) { - if (strcmp(inst->_device_name, device_name) == 0) { - return inst; - } - } - - return nullptr; -} - -int -Mavlink::destroy_all_instances() -{ - /* start deleting from the end */ - Mavlink *inst_to_del = nullptr; - Mavlink *next_inst = ::_mavlink_instances; - - unsigned iterations = 0; - - warnx("waiting for instances to stop"); - - while (next_inst != nullptr) { - inst_to_del = next_inst; - next_inst = inst_to_del->next; - - /* set flag to stop thread and wait for all threads to finish */ - inst_to_del->_task_should_exit = true; - - while (inst_to_del->_task_running) { - printf("."); - fflush(stdout); - usleep(10000); - iterations++; - - if (iterations > 1000) { - warnx("ERROR: Couldn't stop all mavlink instances."); - return ERROR; - } - } - } - - printf("\n"); - warnx("all instances stopped"); - return OK; -} - -int -Mavlink::get_status_all_instances() -{ - Mavlink *inst = ::_mavlink_instances; - - unsigned iterations = 0; - - while (inst != nullptr) { - - printf("\ninstance #%u:\n", iterations); - inst->display_status(); - - /* move on */ - inst = inst->next; - iterations++; - } - - /* return an error if there are no instances */ - return (iterations == 0); -} - -bool -Mavlink::instance_exists(const char *device_name, Mavlink *self) -{ - Mavlink *inst = ::_mavlink_instances; - - while (inst != nullptr) { - - /* don't compare with itself */ - if (inst != self && !strcmp(device_name, inst->_device_name)) { - return true; - } - - inst = inst->next; - } - - return false; -} - -void -Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) -{ - - Mavlink *inst; - LL_FOREACH(_mavlink_instances, inst) { - if (inst != self) { - - /* if not in normal mode, we are an onboard link - * onboard links should only pass on messages from the same system ID */ - if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { - inst->pass_message(msg); - } - } - } -} - -#ifndef __PX4_QURT -int -Mavlink::get_uart_fd(unsigned index) -{ - Mavlink *inst = get_instance(index); - - if (inst) { - return inst->get_uart_fd(); - } - - return -1; -} - -int -Mavlink::get_uart_fd() -{ - return _uart_fd; -} -#endif // __PX4_QURT - -int -Mavlink::get_instance_id() -{ - return _instance_id; -} - -mavlink_channel_t -Mavlink::get_channel() -{ - return _channel; -} - -/**************************************************************************** - * MAVLink text message logger - ****************************************************************************/ - -int -Mavlink::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case (int)MAVLINK_IOC_SEND_TEXT_INFO: - case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: - case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { - - const char *txt = (const char *)arg; - struct mavlink_logmessage msg; - strncpy(msg.text, txt, sizeof(msg.text)); - - switch (cmd) { - case MAVLINK_IOC_SEND_TEXT_INFO: - msg.severity = MAV_SEVERITY_INFO; - break; - - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - msg.severity = MAV_SEVERITY_CRITICAL; - break; - - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - msg.severity = MAV_SEVERITY_EMERGENCY; - break; - - default: - msg.severity = MAV_SEVERITY_INFO; - break; - } - - Mavlink *inst; - LL_FOREACH(_mavlink_instances, inst) { - if (!inst->_task_should_exit) { - mavlink_logbuffer_write(&inst->_logbuffer, &msg); - inst->_total_counter++; - } - } - - return OK; - } - - default: - return ENOTTY; - } -} - -void Mavlink::mavlink_update_system(void) -{ - if (!_param_initialized) { - _param_system_id = param_find("MAV_SYS_ID"); - _param_component_id = param_find("MAV_COMP_ID"); - _param_system_type = param_find("MAV_TYPE"); - _param_use_hil_gps = param_find("MAV_USEHILGPS"); - _param_forward_externalsp = param_find("MAV_FWDEXTSP"); - } - - /* update system and component id */ - int32_t system_id; - param_get(_param_system_id, &system_id); - - int32_t component_id; - param_get(_param_component_id, &component_id); - - - /* only allow system ID and component ID updates - * after reboot - not during operation */ - if (!_param_initialized) { - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; - } - - _param_initialized = true; - } - - /* warn users that they need to reboot to take this - * into effect - */ - if (system_id != mavlink_system.sysid) { - send_statustext_critical("Save params and reboot to change SYSID"); - } - - if (component_id != mavlink_system.compid) { - send_statustext_critical("Save params and reboot to change COMPID"); - } - - int32_t system_type; - param_get(_param_system_type, &system_type); - - if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - _system_type = system_type; - } - - int32_t use_hil_gps; - param_get(_param_use_hil_gps, &use_hil_gps); - - _use_hil_gps = (bool)use_hil_gps; - - int32_t forward_externalsp; - param_get(_param_forward_externalsp, &forward_externalsp); - - _forward_externalsp = (bool)forward_externalsp; -} - -int Mavlink::get_system_id() -{ - return mavlink_system.sysid; -} - -int Mavlink::get_component_id() -{ - return mavlink_system.compid; -} - -#ifndef __PX4_QURT -int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) -{ - /* process baud rate */ - int speed; - - switch (baud) { - case 0: speed = B0; break; - - case 50: speed = B50; break; - - case 75: speed = B75; break; - - case 110: speed = B110; break; - - case 134: speed = B134; break; - - case 150: speed = B150; break; - - case 200: speed = B200; break; - - case 300: speed = B300; break; - - case 600: speed = B600; break; - - case 1200: speed = B1200; break; - - case 1800: speed = B1800; break; - - case 2400: speed = B2400; break; - - case 4800: speed = B4800; break; - - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - case 230400: speed = B230400; break; - - case 460800: speed = B460800; break; - - case 921600: speed = B921600; break; - - default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", - baud); - return -EINVAL; - } - - /* open uart */ - _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); - - if (_uart_fd < 0) { - return _uart_fd; - } - - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; - *is_usb = false; - - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { - warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); - ::close(_uart_fd); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(_uart_fd, &uart_config); - - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* USB serial is indicated by /dev/ttyACM0*/ - if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { - - /* Set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); - ::close(_uart_fd); - return -1; - } - - } - - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { - warnx("ERR SET CONF %s\n", uart_name); - ::close(_uart_fd); - return -1; - } - - if (!_is_usb_uart) { - /* - * Setup hardware flow control. If the port has no RTS pin this call will fail, - * which is not an issue, but requires a separate call so we can fail silently. - */ - (void)tcgetattr(_uart_fd, &uart_config); -#ifdef CRTS_IFLOW - uart_config.c_cflag |= CRTS_IFLOW; -#else - uart_config.c_cflag |= CRTSCTS; -#endif - (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); - - /* setup output flow control */ - if (enable_flow_control(true)) { - warnx("hardware flow control not supported"); - } - - } else { - _flow_control_enabled = false; - } - - return _uart_fd; -} - -int -Mavlink::enable_flow_control(bool enabled) -{ - // We can't do this on USB - skip - if (_is_usb_uart) { - return OK; - } - - struct termios uart_config; - - int ret = tcgetattr(_uart_fd, &uart_config); - - if (enabled) { - uart_config.c_cflag |= CRTSCTS; - - } else { - uart_config.c_cflag &= ~CRTSCTS; - } - - ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); - - if (!ret) { - _flow_control_enabled = enabled; - } - - return ret; -} - -#endif - -int -Mavlink::set_hil_enabled(bool hil_enabled) -{ - int ret = OK; - - /* enable HIL */ - if (hil_enabled && !_hil_enabled) { - _hil_enabled = true; - configure_stream("HIL_CONTROLS", 200.0f); - } - - /* disable HIL */ - if (!hil_enabled && _hil_enabled) { - _hil_enabled = false; - configure_stream("HIL_CONTROLS", 0.0f); - - } else { - ret = ERROR; - } - - return ret; -} - -unsigned -Mavlink::get_free_tx_buf() -{ - /* - * Check if the OS buffer is full and disable HW - * flow control if it continues to be full - */ - int buf_free = 0; - -#ifndef __PX4_QURT - -// No FIONWRITE on Linux -#if !defined(__PX4_LINUX) - (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); -#endif - - if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { - /* 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_time != 0 && - hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && - _last_write_success_time != _last_write_try_time) { - warnx("DISABLING HARDWARE FLOW CONTROL"); - enable_flow_control(false); - } - } -#endif - - return buf_free; -} - -void -Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID) -{ - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (!should_transmit()) { - return; - } - - pthread_mutex_lock(&_send_mutex); - - unsigned buf_free = get_free_tx_buf(); - - uint8_t payload_len = mavlink_message_lengths[msgid]; - unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - - _last_write_try_time = hrt_absolute_time(); - - /* check if there is space in the buffer, let it overflow else */ - if (buf_free < packet_len) { - /* no enough space in buffer to send */ - count_txerr(); - count_txerrbytes(packet_len); - pthread_mutex_unlock(&_send_mutex); - return; - } - - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - /* header */ - buf[0] = MAVLINK_STX; - buf[1] = payload_len; - /* use mavlink's internal counter for the TX seq */ - buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; - buf[3] = mavlink_system.sysid; - buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID; - buf[5] = msgid; - - /* payload */ - memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); - - /* checksum */ - uint16_t checksum; - crc_init(&checksum); - crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); - crc_accumulate(mavlink_message_crcs[msgid], &checksum); - - buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); - buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); - -#ifndef __PX4_QURT - /* send message to UART */ - ssize_t ret = ::write(_uart_fd, buf, packet_len); - - if (ret != (int) packet_len) { - count_txerr(); - count_txerrbytes(packet_len); - - } else { - _last_write_success_time = _last_write_try_time; - count_txbytes(packet_len); - } -#endif - - pthread_mutex_unlock(&_send_mutex); -} - -void -Mavlink::resend_message(mavlink_message_t *msg) -{ - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (!should_transmit()) { - return; - } - - pthread_mutex_lock(&_send_mutex); - - unsigned buf_free = get_free_tx_buf(); - - _last_write_try_time = hrt_absolute_time(); - - unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - - /* check if there is space in the buffer, let it overflow else */ - if (buf_free < packet_len) { - /* no enough space in buffer to send */ - count_txerr(); - count_txerrbytes(packet_len); - pthread_mutex_unlock(&_send_mutex); - return; - } - - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - /* header and payload */ - memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len); - - /* checksum */ - buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); - buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); - -#ifndef __PX4_QURT - /* send message to UART */ - ssize_t ret = ::write(_uart_fd, buf, packet_len); - - if (ret != (int) packet_len) { - count_txerr(); - count_txerrbytes(packet_len); - - } else { - _last_write_success_time = _last_write_try_time; - count_txbytes(packet_len); - } -#endif - - pthread_mutex_unlock(&_send_mutex); -} - -void -Mavlink::handle_message(const mavlink_message_t *msg) -{ - /* handle packet with mission manager */ - _mission_manager->handle_message(msg); - - /* handle packet with parameter component */ - _parameters_manager->handle_message(msg); - - if (get_forwarding_on()) { - /* forward any messages to other mavlink instances */ - Mavlink::forward_message(msg, this); - } -} - -void -Mavlink::send_statustext_info(const char *string) -{ - send_statustext(MAV_SEVERITY_INFO, string); -} - -void -Mavlink::send_statustext_critical(const char *string) -{ - send_statustext(MAV_SEVERITY_CRITICAL, string); -} - -void -Mavlink::send_statustext_emergency(const char *string) -{ - send_statustext(MAV_SEVERITY_EMERGENCY, string); -} - -void -Mavlink::send_statustext(unsigned char severity, const char *string) -{ - struct mavlink_logmessage logmsg; - strncpy(logmsg.text, string, sizeof(logmsg.text)); - logmsg.severity = severity; - - mavlink_logbuffer_write(&_logbuffer, &logmsg); -} - -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance) -{ - /* check if already subscribed to this topic */ - MavlinkOrbSubscription *sub; - - LL_FOREACH(_subscriptions, sub) { - if (sub->get_topic() == topic && sub->get_instance() == instance) { - /* already subscribed */ - return sub; - } - } - - /* add new subscription */ - MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance); - - LL_APPEND(_subscriptions, sub_new); - - return sub_new; -} - -unsigned int -Mavlink::interval_from_rate(float rate) -{ - return (rate > 0.0f) ? (1000000.0f / rate) : 0; -} - -int -Mavlink::configure_stream(const char *stream_name, const float rate) -{ - /* calculate interval in us, 0 means disabled stream */ - unsigned int interval = interval_from_rate(rate); - - /* search if stream exists */ - MavlinkStream *stream; - LL_FOREACH(_streams, stream) { - if (strcmp(stream_name, stream->get_name()) == 0) { - if (interval > 0) { - /* set new interval */ - stream->set_interval(interval); - - } else { - /* delete stream */ - LL_DELETE(_streams, stream); - delete stream; - } - - return OK; - } - } - - if (interval == 0) { - /* stream was not active and is requested to be disabled, do nothing */ - return OK; - } - - /* search for stream with specified name in supported streams list */ - for (unsigned int i = 0; streams_list[i] != nullptr; i++) { - - if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { - /* create new instance */ - stream = streams_list[i]->new_instance(this); - stream->set_interval(interval); - LL_APPEND(_streams, stream); - - return OK; - } - } - - /* if we reach here, the stream list does not contain the stream */ - warnx("stream %s not found", stream_name); - - return ERROR; -} - -void -Mavlink::adjust_stream_rates(const float multiplier) -{ - /* do not allow to push us to zero */ - if (multiplier < 0.01f) { - return; - } - - /* search if stream exists */ - MavlinkStream *stream; - LL_FOREACH(_streams, stream) { - /* set new interval */ - unsigned interval = stream->get_interval(); - interval /= multiplier; - - /* allow max ~600 Hz */ - if (interval < 1600) { - interval = 1600; - } - - /* set new interval */ - stream->set_interval(interval * multiplier); - } -} - -void -Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) -{ - /* orb subscription must be done from the main thread, - * set _subscribe_to_stream and _subscribe_to_stream_rate fields - * which polled in mavlink main loop */ - if (!_task_should_exit) { - /* wait for previous subscription completion */ - while (_subscribe_to_stream != nullptr) { - usleep(MAIN_LOOP_DELAY / 2); - } - - /* copy stream name */ - unsigned n = strlen(stream_name) + 1; - char *s = new char[n]; - strcpy(s, stream_name); - - /* set subscription task */ - _subscribe_to_stream_rate = rate; - _subscribe_to_stream = s; - - /* wait for subscription */ - do { - usleep(MAIN_LOOP_DELAY / 2); - } while (_subscribe_to_stream != nullptr); - - delete s; - } -} - -int -Mavlink::message_buffer_init(int size) -{ - - _message_buffer.size = size; - _message_buffer.write_ptr = 0; - _message_buffer.read_ptr = 0; - _message_buffer.data = (char *)malloc(_message_buffer.size); - - int ret; - - if (_message_buffer.data == 0) { - ret = ERROR; - _message_buffer.size = 0; - - } else { - ret = OK; - } - - return ret; -} - -void -Mavlink::message_buffer_destroy() -{ - _message_buffer.size = 0; - _message_buffer.write_ptr = 0; - _message_buffer.read_ptr = 0; - free(_message_buffer.data); -} - -int -Mavlink::message_buffer_count() -{ - int n = _message_buffer.write_ptr - _message_buffer.read_ptr; - - if (n < 0) { - n += _message_buffer.size; - } - - return n; -} - -int -Mavlink::message_buffer_is_empty() -{ - return _message_buffer.read_ptr == _message_buffer.write_ptr; -} - - -bool -Mavlink::message_buffer_write(const void *ptr, int size) -{ - // bytes available to write - int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; - - if (available < 0) { - available += _message_buffer.size; - } - - if (size > available) { - // buffer overflow - return false; - } - - char *c = (char *) ptr; - int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer - - if (n < size) { - // message goes over end of the buffer - memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); - _message_buffer.write_ptr = 0; - - } else { - n = 0; - } - - // now: n = bytes already written - int p = size - n; // number of bytes to write - memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); - _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; - return true; -} - -int -Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) -{ - // bytes available to read - int available = _message_buffer.write_ptr - _message_buffer.read_ptr; - - if (available == 0) { - return 0; // buffer is empty - } - - int n = 0; - - if (available > 0) { - // read pointer is before write pointer, all available bytes can be read - n = available; - *is_part = false; - - } else { - // read pointer is after write pointer, read bytes from read_ptr to end of the buffer - n = _message_buffer.size - _message_buffer.read_ptr; - *is_part = _message_buffer.write_ptr > 0; - } - - *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); - return n; -} - -void -Mavlink::message_buffer_mark_read(int n) -{ - _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; -} - -void -Mavlink::pass_message(const mavlink_message_t *msg) -{ - if (_passing_on) { - /* size is 8 bytes plus variable payload */ - int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; - pthread_mutex_lock(&_message_buffer_mutex); - message_buffer_write(msg, size); - pthread_mutex_unlock(&_message_buffer_mutex); - } -} - -float -Mavlink::get_rate_mult() -{ - return _rate_mult; -} - -void -Mavlink::update_rate_mult() -{ - float const_rate = 0.0f; - float rate = 0.0f; - - MavlinkStream *stream; - LL_FOREACH(_streams, stream) { - if (stream->const_rate()) { - const_rate += stream->get_size() * 1000000.0f / stream->get_interval(); - - } else { - rate += stream->get_size() * 1000000.0f / stream->get_interval(); - } - } - - /* don't scale up rates, only scale down if needed */ - _rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); -} - -int -Mavlink::task_main(int argc, char *argv[]) -{ - int ch; - _baudrate = 57600; - _datarate = 0; - _mode = MAVLINK_MODE_NORMAL; - - /* don't exit from getopt loop to leave getopt global variables in consistent state, - * set error flag instead */ - bool err_flag = false; - int myoptind=1; - const char *myoptarg = NULL; - - while ((ch = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'b': - _baudrate = strtoul(myoptarg, NULL, 10); - - if (_baudrate < 9600 || _baudrate > 921600) { - warnx("invalid baud rate '%s'", myoptarg); - err_flag = true; - } - - break; - - case 'r': - _datarate = strtoul(myoptarg, NULL, 10); - - if (_datarate < 10 || _datarate > MAX_DATA_RATE) { - warnx("invalid data rate '%s'", myoptarg); - err_flag = true; - } - - break; - - case 'd': - _device_name = myoptarg; - break; - -// case 'e': -// mavlink_link_termination_allowed = true; -// break; - - case 'm': - if (strcmp(myoptarg, "custom") == 0) { - _mode = MAVLINK_MODE_CUSTOM; - - } else if (strcmp(myoptarg, "camera") == 0) { - // left in here for compatibility - _mode = MAVLINK_MODE_ONBOARD; - } else if (strcmp(myoptarg, "onboard") == 0) { - _mode = MAVLINK_MODE_ONBOARD; - } - - break; - - case 'f': - _forwarding_on = true; - break; - - case 'p': - _passing_on = true; - break; - - case 'v': - _verbose = true; - break; - - case 'w': - _wait_to_transmit = true; - break; - - case 'x': - _ftp_on = true; - break; - - default: - err_flag = true; - break; - } - } - - if (err_flag) { - usage(); - return ERROR; - } - - if (_datarate == 0) { - /* convert bits to bytes and use 1/2 of bandwidth by default */ - _datarate = _baudrate / 20; - } - - if (_datarate > MAX_DATA_RATE) { - _datarate = MAX_DATA_RATE; - } - - if (Mavlink::instance_exists(_device_name, this)) { - warnx("%s already running", _device_name); - return ERROR; - } - - warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); - - /* flush stdout in case MAVLink is about to take it over */ - fflush(stdout); - -#ifndef __PX4_QURT - struct termios uart_config_original; - - /* default values for arguments */ - _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart); - - if (_uart_fd < 0) { - warn("could not open %s", _device_name); - return ERROR; - } -#endif - - /* initialize send mutex */ - pthread_mutex_init(&_send_mutex, NULL); - - /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&_logbuffer, 5); - - /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ - if (_passing_on || _ftp_on) { - /* initialize message buffer if multiplexing is on or its needed for FTP. - * make space for two messages plus off-by-one space as we use the empty element - * marker ring buffer approach. - */ - if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { - warnx("msg buf:"); - return 1; - } - - /* initialize message buffer mutex */ - pthread_mutex_init(&_message_buffer_mutex, NULL); - } - - /* create the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, NULL); - - /* initialize logging device */ - _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); - - /* Initialize system properties */ - mavlink_update_system(); - - /* start the MAVLink receiver */ - _receive_thread = MavlinkReceiver::receive_start(this); - - _task_running = true; - - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); - uint64_t param_time = 0; - MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); - uint64_t status_time = 0; - - struct vehicle_status_s status; - status_sub->update(&status_time, &status); - - /* add default streams depending on mode */ - - /* HEARTBEAT is constant rate stream, rate never adjusted */ - configure_stream("HEARTBEAT", 1.0f); - - /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */ - configure_stream("STATUSTEXT", 20.0f); - - /* COMMAND_LONG stream: use high rate to avoid commands skipping */ - configure_stream("COMMAND_LONG", 100.0f); - - /* PARAM_VALUE stream */ - _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); - _parameters_manager->set_interval(interval_from_rate(30.0f)); - LL_APPEND(_streams, _parameters_manager); - - /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on - * remote requests rate. Rate specified here controls how much bandwidth we will reserve for - * mission messages. */ - _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); - _mission_manager->set_interval(interval_from_rate(10.0f)); - _mission_manager->set_verbose(_verbose); - LL_APPEND(_streams, _mission_manager); - - switch (_mode) { - case MAVLINK_MODE_NORMAL: - configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); - configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 10.0f); - configure_stream("VFR_HUD", 8.0f); - configure_stream("GPS_RAW_INT", 1.0f); - configure_stream("GLOBAL_POSITION_INT", 3.0f); - configure_stream("LOCAL_POSITION_NED", 3.0f); - configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); - configure_stream("ATTITUDE_TARGET", 3.0f); - configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW_RAD", 5.0f); - break; - - case MAVLINK_MODE_ONBOARD: - configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 50.0f); - configure_stream("GLOBAL_POSITION_INT", 50.0f); - configure_stream("LOCAL_POSITION_NED", 30.0f); - configure_stream("CAMERA_CAPTURE", 2.0f); - configure_stream("ATTITUDE_TARGET", 10.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); - configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); - configure_stream("DISTANCE_SENSOR", 10.0f); - configure_stream("OPTICAL_FLOW_RAD", 10.0f); - configure_stream("VFR_HUD", 10.0f); - configure_stream("SYSTEM_TIME", 1.0f); - configure_stream("TIMESYNC", 10.0f); - configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); - break; - - default: - break; - } - - /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate; - - /* now the instance is fully initialized and we can bump the instance count */ - LL_APPEND(_mavlink_instances, this); - - while (!_task_should_exit) { - /* main loop */ - usleep(_main_loop_delay); - - perf_begin(_loop_perf); - - hrt_abstime t = hrt_absolute_time(); - - update_rate_mult(); - - if (param_sub->update(¶m_time, nullptr)) { - /* parameters updated */ - mavlink_update_system(); - } - - if (status_sub->update(&status_time, &status)) { - /* switch HIL mode if required */ - set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); - } - - /* check for requested subscriptions */ - if (_subscribe_to_stream != nullptr) { - if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { - if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, - (double)_subscribe_to_stream_rate); - - } else { - warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); - } - - } else { - warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); - } - - _subscribe_to_stream = nullptr; - } - - /* update streams */ - MavlinkStream *stream; - LL_FOREACH(_streams, stream) { - stream->update(t); - } - - /* pass messages from other UARTs or FTP worker */ - if (_passing_on || _ftp_on) { - - bool is_part; - uint8_t *read_ptr; - uint8_t *write_ptr; - - pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); - pthread_mutex_unlock(&_message_buffer_mutex); - - if (available > 0) { - // Reconstruct message from buffer - - mavlink_message_t msg; - write_ptr = (uint8_t *)&msg; - - // Pull a single message from the buffer - size_t read_count = available; - - if (read_count > sizeof(mavlink_message_t)) { - read_count = sizeof(mavlink_message_t); - } - - memcpy(write_ptr, read_ptr, read_count); - - // We hold the mutex until after we complete the second part of the buffer. If we don't - // we may end up breaking the empty slot overflow detection semantics when we mark the - // possibly partial read below. - pthread_mutex_lock(&_message_buffer_mutex); - - message_buffer_mark_read(read_count); - - /* write second part of buffer if there is some */ - if (is_part && read_count < sizeof(mavlink_message_t)) { - write_ptr += read_count; - available = message_buffer_get_ptr((void **)&read_ptr, &is_part); - read_count = sizeof(mavlink_message_t) - read_count; - memcpy(write_ptr, read_ptr, read_count); - message_buffer_mark_read(available); - } - - pthread_mutex_unlock(&_message_buffer_mutex); - - resend_message(&msg); - } - } - - /* update TX/RX rates*/ - if (t > _bytes_timestamp + 1000000) { - if (_bytes_timestamp != 0) { - float dt = (t - _bytes_timestamp) / 1000.0f; - _rate_tx = _bytes_tx / dt; - _rate_txerr = _bytes_txerr / dt; - _rate_rx = _bytes_rx / dt; - _bytes_tx = 0; - _bytes_txerr = 0; - _bytes_rx = 0; - } - - _bytes_timestamp = t; - } - - perf_end(_loop_perf); - } - - delete _subscribe_to_stream; - _subscribe_to_stream = nullptr; - - /* delete streams */ - MavlinkStream *stream_to_del = nullptr; - MavlinkStream *stream_next = _streams; - - while (stream_next != nullptr) { - stream_to_del = stream_next; - stream_next = stream_to_del->next; - delete stream_to_del; - } - - _streams = nullptr; - - /* delete subscriptions */ - MavlinkOrbSubscription *sub_to_del = nullptr; - MavlinkOrbSubscription *sub_next = _subscriptions; - - while (sub_next != nullptr) { - sub_to_del = sub_next; - sub_next = sub_to_del->next; - delete sub_to_del; - } - - _subscriptions = nullptr; - - /* wait for threads to complete */ - pthread_join(_receive_thread, NULL); - -#ifndef __PX4_QURT - /* reset the UART flags to original state */ - tcsetattr(_uart_fd, TCSANOW, &uart_config_original); - - /* close UART */ - ::close(_uart_fd); -#endif - - /* close mavlink logging device */ - px4_close(_mavlink_fd); - - if (_passing_on || _ftp_on) { - message_buffer_destroy(); - pthread_mutex_destroy(&_message_buffer_mutex); - } - - /* destroy log buffer */ - mavlink_logbuffer_destroy(&_logbuffer); - - warnx("exiting"); - _task_running = false; - - return OK; -} - -int Mavlink::start_helper(int argc, char *argv[]) -{ - /* create the instance in task context */ - Mavlink *instance = new Mavlink(); - - int res; - - if (!instance) { - - /* out of memory */ - res = -ENOMEM; - warnx("OUT OF MEM"); - - } else { - /* this will actually only return once MAVLink exits */ - res = instance->task_main(argc, argv); - - /* delete instance on main thread end */ - delete instance; - } - - return res; -} - -int -Mavlink::start(int argc, char *argv[]) -{ - // Wait for the instance count to go up one - // before returning to the shell - int ic = Mavlink::instance_count(); - - // Instantiate thread - char buf[24]; - sprintf(buf, "mavlink_if%d", ic); - - // This is where the control flow splits - // between the starting task and the spawned - // task - start_helper() only returns - // when the started task exits. - px4_task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2400, - (px4_main_t)&Mavlink::start_helper, - (char * const *)argv); - - // Ensure that this shell command - // does not return before the instance - // is fully initialized. As this is also - // the only path to create a new instance, - // this is effectively a lock on concurrent - // instance starting. XXX do a real lock. - - // Sleep 500 us between each attempt - const unsigned sleeptime = 500; - - // Wait 100 ms max for the startup. - const unsigned limit = 100 * 1000 / sleeptime; - - unsigned count = 0; - - while (ic == Mavlink::instance_count() && count < limit) { - ::usleep(sleeptime); - count++; - } - - return OK; -} - -void -Mavlink::display_status() -{ - - if (_rstatus.heartbeat_time > 0) { - printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_rstatus.heartbeat_time)); - } - - printf("\tmavlink chan: #%u\n", _channel); - - if (_rstatus.timestamp > 0) { - - printf("\ttype:\t\t"); - - switch (_rstatus.type) { - case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: - printf("3DR RADIO\n"); - break; - - default: - printf("UNKNOWN RADIO\n"); - break; - } - - printf("\trssi:\t\t%d\n", _rstatus.rssi); - printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); - printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); - printf("\tnoise:\t\t%d\n", _rstatus.noise); - printf("\tremote noise:\t%u\n", _rstatus.remote_noise); - printf("\trx errors:\t%u\n", _rstatus.rxerrors); - printf("\tfixed:\t\t%u\n", _rstatus.fixed); - - } else { - printf("\tno telem status.\n"); - } - - printf("\trates:\n"); - printf("\ttx: %.3f kB/s\n", (double)_rate_tx); - printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); - printf("\trx: %.3f kB/s\n", (double)_rate_rx); - printf("\trate mult: %.3f\n", (double)_rate_mult); -} - -int -Mavlink::stream_command(int argc, char *argv[]) -{ - const char *device_name = DEFAULT_DEVICE_NAME; - float rate = -1.0f; - const char *stream_name = nullptr; - - argc -= 2; - argv += 2; - - /* don't exit from getopt loop to leave getopt global variables in consistent state, - * set error flag instead */ - bool err_flag = false; - - int i = 0; - - while (i < argc) { - - if (0 == strcmp(argv[i], "-r") && i < argc - 1) { - rate = strtod(argv[i + 1], nullptr); - - if (rate < 0.0f) { - err_flag = true; - } - - i++; - - } else if (0 == strcmp(argv[i], "-d") && i < argc - 1) { - device_name = argv[i + 1]; - i++; - - } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { - stream_name = argv[i + 1]; - i++; - - } else { - err_flag = true; - } - - i++; - } - - if (!err_flag && rate >= 0.0f && stream_name != nullptr) { - Mavlink *inst = get_instance_for_device(device_name); - - if (inst != nullptr) { - inst->configure_stream_threadsafe(stream_name, rate); - - } else { - - // If the link is not running we should complain, but not fall over - // because this is so easy to get wrong and not fatal. Warning is sufficient. - warnx("mavlink for device %s is not running", device_name); - return 0; - } - - } else { - warnx("usage: mavlink stream [-d device] -s stream -r rate"); - return 1; - } - - return OK; -} - -static void usage() -{ - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); -} - -int mavlink_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(); - return 1; - } - - if (!strcmp(argv[1], "start")) { - return Mavlink::start(argc, argv); - - } else if (!strcmp(argv[1], "stop")) { - warnx("mavlink stop is deprecated, use stop-all instead"); - usage(); - return 1; - - } else if (!strcmp(argv[1], "stop-all")) { - return Mavlink::destroy_all_instances(); - - } else if (!strcmp(argv[1], "status")) { - return Mavlink::get_status_all_instances(); - - } else if (!strcmp(argv[1], "stream")) { - return Mavlink::stream_command(argc, argv); - - } else { - usage(); - return 1; - } - - return 0; -} diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1b2689e6bbf9..be3ab850f418 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -52,9 +53,10 @@ #include #include #include +#include #include #include -#include +#include #include #include #include @@ -69,6 +71,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -131,6 +136,12 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; + case vehicle_status_s::NAVIGATION_STATE_STAB: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED; + break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; @@ -389,7 +400,7 @@ class MavlinkStreamStatustext : public MavlinkStream } else if (write_err_count < write_err_threshold) { /* string to hold the path to the log */ char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_path[70] = ""; timespec ts; px4_clock_gettime(CLOCK_REALTIME, &ts); @@ -400,12 +411,17 @@ class MavlinkStreamStatustext : public MavlinkStream // 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", &tt); - snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); + snprintf(log_file_path, sizeof(log_file_path), PX4_ROOTFSDIR"/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); - /* write first message */ - fputs(msg.text, fp); - fputs("\n", fp); + if (fp != NULL) { + /* write first message */ + fputs(msg.text, fp); + fputs("\n", fp); + } + else { + warn("Failed to open %s errno=%d", log_file_path, errno); + } } } } @@ -541,7 +557,7 @@ class MavlinkStreamSysStatus : public MavlinkStream msg.errors_count2 = status.errors_count2; msg.errors_count3 = status.errors_count3; msg.errors_count4 = status.errors_count4; - msg.battery_remaining = (msg.voltage_battery > 0) ? + msg.battery_remaining = (status.condition_battery_voltage_valid) ? status.battery_remaining * 100.0f : -1; _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); @@ -556,14 +572,22 @@ class MavlinkStreamSysStatus : public MavlinkStream if (i < status.battery_cell_count) { bat_msg.voltages[i] = (status.battery_voltage / status.battery_cell_count) * 1000.0f; } else { - bat_msg.voltages[i] = 0; + bat_msg.voltages[i] = UINT16_MAX; } } - bat_msg.current_battery = status.battery_current * 100.0f; - bat_msg.current_consumed = status.battery_discharged_mah; + + if (status.condition_battery_voltage_valid) { + bat_msg.current_battery = (bat_msg.current_battery >= 0.0f) ? + status.battery_current * 100.0f : -1; + bat_msg.current_consumed = (bat_msg.current_consumed >= 0.0f) ? + status.battery_discharged_mah : -1; + bat_msg.battery_remaining = status.battery_remaining * 100.0f; + } else { + bat_msg.current_battery = -1.0f; + bat_msg.current_consumed = -1.0f; + bat_msg.battery_remaining = -1.0f; + } bat_msg.energy_consumed = -1.0f; - bat_msg.battery_remaining = (status.battery_voltage > 0) ? - status.battery_remaining * 100.0f : -1; _mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg); } @@ -629,28 +653,28 @@ class MavlinkStreamHighresIMU : public MavlinkStream if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (_accel_timestamp != sensor.accelerometer_timestamp) { + if (_accel_timestamp != sensor.accelerometer_timestamp[0]) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - _accel_timestamp = sensor.accelerometer_timestamp; + _accel_timestamp = sensor.accelerometer_timestamp[0]; } - if (_gyro_timestamp != sensor.timestamp) { + if (_gyro_timestamp != sensor.gyro_timestamp[0]) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - _gyro_timestamp = sensor.timestamp; + _gyro_timestamp = sensor.gyro_timestamp[0]; } - if (_mag_timestamp != sensor.magnetometer_timestamp) { + if (_mag_timestamp != sensor.magnetometer_timestamp[0]) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - _mag_timestamp = sensor.magnetometer_timestamp; + _mag_timestamp = sensor.magnetometer_timestamp[0]; } - if (_baro_timestamp != sensor.baro_timestamp) { + if (_baro_timestamp != sensor.baro_timestamp[0]) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - _baro_timestamp = sensor.baro_timestamp; + _baro_timestamp = sensor.baro_timestamp[0]; } mavlink_highres_imu_t msg; @@ -665,10 +689,10 @@ class MavlinkStreamHighresIMU : public MavlinkStream msg.xmag = sensor.magnetometer_ga[0]; msg.ymag = sensor.magnetometer_ga[1]; msg.zmag = sensor.magnetometer_ga[2]; - msg.abs_pressure = sensor.baro_pres_mbar; - msg.diff_pressure = sensor.differential_pressure_pa; - msg.pressure_alt = sensor.baro_alt_meter; - msg.temperature = sensor.baro_temp_celcius; + msg.abs_pressure = sensor.baro_pres_mbar[0]; + msg.diff_pressure = sensor.differential_pressure_pa[0]; + msg.pressure_alt = sensor.baro_alt_meter[0]; + msg.temperature = sensor.baro_temp_celcius[0]; msg.fields_updated = fields_updated; _mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg); @@ -1050,6 +1074,67 @@ class MavlinkStreamTimesync : public MavlinkStream } }; + +class MavlinkStreamCameraTrigger : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCameraTrigger::get_name_static(); + } + + static const char *get_name_static() + { + return "CAMERA_TRIGGER"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_CAMERA_TRIGGER; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamCameraTrigger(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_trigger_sub; + uint64_t _trigger_time; + + /* do not allow top copying this class */ + MavlinkStreamCameraTrigger(MavlinkStreamCameraTrigger &); + MavlinkStreamCameraTrigger& operator = (const MavlinkStreamCameraTrigger &); + +protected: + explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink), + _trigger_sub(_mavlink->add_orb_subscription(ORB_ID(camera_trigger))), + _trigger_time(0) + {} + + void send(const hrt_abstime t) + { + struct camera_trigger_s trigger; + + if (_trigger_sub->update(&_trigger_time, &trigger)) { + mavlink_camera_trigger_t msg; + + msg.time_usec = trigger.timestamp; + msg.seq = trigger.seq; + + /* ensure that only active trigger events are sent */ + if (trigger.timestamp > 0) { + _mavlink->send_message(MAVLINK_MSG_ID_CAMERA_TRIGGER, &msg); + } + } + } +}; + class MavlinkStreamGlobalPositionInt : public MavlinkStream { public: @@ -1187,106 +1272,179 @@ class MavlinkStreamLocalPositionNED : public MavlinkStream }; -class MavlinkStreamViconPositionEstimate : public MavlinkStream +class MavlinkStreamLocalPositionNEDCOV : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamViconPositionEstimate::get_name_static(); + return MavlinkStreamLocalPositionNEDCOV::get_name_static(); } static const char *get_name_static() { - return "VICON_POSITION_ESTIMATE"; + return "LOCAL_POSITION_NED_COV"; } uint8_t get_id() { - return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamViconPositionEstimate(mavlink); + return new MavlinkStreamLocalPositionNEDCOV(mavlink); } unsigned get_size() { - return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *_pos_sub; - uint64_t _pos_time; + MavlinkOrbSubscription *_est_sub; + uint64_t _est_time; /* do not allow top copying this class */ - MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); - MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); + MavlinkStreamLocalPositionNEDCOV(MavlinkStreamLocalPositionNEDCOV &); + MavlinkStreamLocalPositionNEDCOV& operator = (const MavlinkStreamLocalPositionNEDCOV &); protected: - explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), - _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))), - _pos_time(0) + explicit MavlinkStreamLocalPositionNEDCOV(Mavlink *mavlink) : MavlinkStream(mavlink), + _est_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))), + _est_time(0) {} void send(const hrt_abstime t) { - struct vehicle_vicon_position_s pos; + struct estimator_status_s est; + + if (_est_sub->update(&_est_time, &est)) { + mavlink_local_position_ned_cov_t msg; + + msg.time_boot_ms = est.timestamp / 1000; + msg.x = est.states[0]; + msg.y = est.states[1]; + msg.z = est.states[2]; + msg.vx = est.states[3]; + msg.vy = est.states[4]; + msg.vz = est.states[5]; + msg.ax = est.states[6]; + msg.ay = est.states[7]; + msg.az = est.states[8]; + for (int i=0;i<9;i++) { + msg.covariance[i] = est.covariances[i]; + } + msg.covariance[9] = est.nan_flags; + msg.covariance[10] = est.health_flags; + msg.covariance[11] = est.timeout_flags; + memcpy(msg.covariance, est.covariances, sizeof(est.covariances)); - if (_pos_sub->update(&_pos_time, &pos)) { - mavlink_vicon_position_estimate_t msg; + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, &msg); + } + } +}; - msg.usec = pos.timestamp; - msg.x = pos.x; - msg.y = pos.y; - msg.z = pos.z; - msg.roll = pos.roll; - msg.pitch = pos.pitch; - msg.yaw = pos.yaw; +class MavlinkStreamAttPosMocap : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAttPosMocap::get_name_static(); + } + + static const char *get_name_static() + { + return "ATT_POS_MOCAP"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATT_POS_MOCAP; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttPosMocap(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_mocap_sub; + uint64_t _mocap_time; + + /* do not allow top copying this class */ + MavlinkStreamAttPosMocap(MavlinkStreamAttPosMocap &); + MavlinkStreamAttPosMocap& operator = (const MavlinkStreamAttPosMocap &); + +protected: + explicit MavlinkStreamAttPosMocap(Mavlink *mavlink) : MavlinkStream(mavlink), + _mocap_sub(_mavlink->add_orb_subscription(ORB_ID(att_pos_mocap))), + _mocap_time(0) + {} - _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); + void send(const hrt_abstime t) + { + struct att_pos_mocap_s mocap; + + if (_mocap_sub->update(&_mocap_time, &mocap)) { + mavlink_att_pos_mocap_t msg; + + msg.time_usec = mocap.timestamp_boot; + msg.q[0] = mocap.q[0]; + msg.q[1] = mocap.q[1]; + msg.q[2] = mocap.q[2]; + msg.q[3] = mocap.q[3]; + msg.x = mocap.x; + msg.y = mocap.y; + msg.z = mocap.z; + + _mavlink->send_message(MAVLINK_MSG_ID_ATT_POS_MOCAP, &msg); } } }; -class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +class MavlinkStreamHomePosition : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamGPSGlobalOrigin::get_name_static(); + return MavlinkStreamHomePosition::get_name_static(); } static const char *get_name_static() { - return "GPS_GLOBAL_ORIGIN"; + return "HOME_POSITION"; } uint8_t get_id() { - return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; + return MAVLINK_MSG_ID_HOME_POSITION; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGPSGlobalOrigin(mavlink); + return new MavlinkStreamHomePosition(mavlink); } unsigned get_size() { - return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _home_sub->is_published() ? (MAVLINK_MSG_ID_HOME_POSITION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: MavlinkOrbSubscription *_home_sub; /* do not allow top copying this class */ - MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); - MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamHomePosition(MavlinkStreamHomePosition &); + MavlinkStreamHomePosition& operator = (const MavlinkStreamHomePosition &); protected: - explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink), + explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink), _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) {} @@ -1298,13 +1456,26 @@ class MavlinkStreamGPSGlobalOrigin : public MavlinkStream struct home_position_s home; if (_home_sub->update(&home)) { - mavlink_gps_global_origin_t msg; + mavlink_home_position_t msg; msg.latitude = home.lat * 1e7; msg.longitude = home.lon * 1e7; msg.altitude = home.alt * 1e3f; - _mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg); + msg.x = home.x; + msg.y = home.y; + msg.z = home.z; + + msg.q[0] = 1.0f; + msg.q[1] = 0.0f; + msg.q[2] = 0.0f; + msg.q[3] = 0.0f; + + msg.approach_x = 0.0f; + msg.approach_y = 0.0f; + msg.approach_z = 0.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_HOME_POSITION, &msg); } } } @@ -1764,6 +1935,10 @@ class MavlinkStreamLocalPositionSetpoint : public MavlinkStream msg.x = pos_sp.x; msg.y = pos_sp.y; msg.z = pos_sp.z; + msg.yaw = pos_sp.yaw; + msg.vx = pos_sp.vx; + msg.vy = pos_sp.vy; + msg.vz = pos_sp.vz; _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } @@ -1843,33 +2018,32 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream }; -class MavlinkStreamRCChannelsRaw : public MavlinkStream +class MavlinkStreamRCChannels : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamRCChannelsRaw::get_name_static(); + return MavlinkStreamRCChannels::get_name_static(); } static const char *get_name_static() { - return "RC_CHANNELS_RAW"; + return "RC_CHANNELS"; } uint8_t get_id() { - return MAVLINK_MSG_ID_RC_CHANNELS_RAW; + return MAVLINK_MSG_ID_RC_CHANNELS; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamRCChannelsRaw(mavlink); + return new MavlinkStreamRCChannels(mavlink); } unsigned get_size() { - return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + - MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _rc_sub->is_published() ? (MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1877,11 +2051,11 @@ class MavlinkStreamRCChannelsRaw : public MavlinkStream uint64_t _rc_time; /* do not allow top copying this class */ - MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); - MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannels(MavlinkStreamRCChannels &); + MavlinkStreamRCChannels& operator = (const MavlinkStreamRCChannels &); protected: - explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + explicit MavlinkStreamRCChannels(Mavlink *mavlink) : MavlinkStream(mavlink), _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))), _rc_time(0) {} @@ -1978,7 +2152,14 @@ class MavlinkStreamManualControl : public MavlinkStream msg.y = manual.y * 1000; msg.z = manual.z * 1000; msg.r = manual.r * 1000; + unsigned shift = 2; msg.buttons = 0; + msg.buttons |= (manual.mode_switch << (shift * 0)); + msg.buttons |= (manual.return_switch << (shift * 1)); + msg.buttons |= (manual.posctl_switch << (shift * 2)); + msg.buttons |= (manual.loiter_switch << (shift * 3)); + msg.buttons |= (manual.acro_switch << (shift * 4)); + msg.buttons |= (manual.offboard_switch << (shift * 5)); _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); } @@ -2262,6 +2443,99 @@ class MavlinkStreamDistanceSensor : public MavlinkStream } }; +class MavlinkStreamExtendedSysState : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamExtendedSysState::get_name_static(); + } + + static const char *get_name_static() + { + return "EXTENDED_SYS_STATE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_EXTENDED_SYS_STATE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamExtendedSysState(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_status_sub; + MavlinkOrbSubscription *_landed_sub; + mavlink_extended_sys_state_t _msg; + + /* do not allow top copying this class */ + MavlinkStreamExtendedSysState(MavlinkStreamExtendedSysState &); + MavlinkStreamExtendedSysState &operator = (const MavlinkStreamExtendedSysState &); + +protected: + explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _landed_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))), + _msg{} + { + + _msg.vtol_state = MAV_VTOL_STATE_UNDEFINED; + _msg.landed_state = MAV_LANDED_STATE_UNDEFINED; + } + + void send(const hrt_abstime t) + { + struct vehicle_status_s status; + struct vehicle_land_detected_s land_detected; + bool updated = false; + + if (_status_sub->update(&status)) { + updated = true; + + if (status.is_vtol) { + if (status.is_rotary_wing) { + if (status.in_transition_mode) { + _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW; + + } else { + _msg.vtol_state = MAV_VTOL_STATE_MC; + } + + } else { + if (status.in_transition_mode) { + _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC; + + } else { + _msg.vtol_state = MAV_VTOL_STATE_FW; + } + } + } + } + + if (_landed_sub->update(&land_detected)) { + updated = true; + + if (land_detected.landed) { + _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; + + } else { + _msg.landed_state = MAV_LANDED_STATE_IN_AIR; + } + } + + if (updated) { + _mavlink->send_message(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, &_msg); + } + } +}; const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), @@ -2277,8 +2551,9 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), - new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), - new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), + new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static), + new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static), + new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), @@ -2287,7 +2562,7 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static), - new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), + new StreamListItem(&MavlinkStreamRCChannels::new_instance, &MavlinkStreamRCChannels::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static), new StreamListItem(&MavlinkStreamActuatorControlTarget<0>::new_instance, &MavlinkStreamActuatorControlTarget<0>::get_name_static), @@ -2296,6 +2571,8 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static), new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), + new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static), new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), + new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static), nullptr }; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index bc0b269ea17e..bb0908908cbf 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,9 +35,9 @@ * @file mavlink_mission.cpp * MAVLink mission manager implementation. * - * @author Lorenz Meier - * @author Julian Oes - * @author Anton Babushkin + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin */ #include "mavlink_mission.h" @@ -49,6 +49,7 @@ #include #include +#include #include #include @@ -58,6 +59,12 @@ #endif static const int ERROR = -1; +int MavlinkMissionManager::_dataman_id = 0; +bool MavlinkMissionManager::_dataman_init = false; +unsigned MavlinkMissionManager::_count = 0; +int MavlinkMissionManager::_current_seq = 0; +bool MavlinkMissionManager::_transfer_in_progress = false; + #define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ ((_msg.target_component == mavlink_system.compid) || \ (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ @@ -70,9 +77,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), - _dataman_id(0), - _count(0), - _current_seq(0), + _filesystem_errcount(0), + _my_dataman_id(0), _transfer_dataman_id(0), _transfer_count(0), _transfer_seq(0), @@ -114,17 +120,21 @@ void MavlinkMissionManager::init_offboard_mission() { mission_s mission_state; - if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) { - _dataman_id = mission_state.dataman_id; - _count = mission_state.count; - _current_seq = mission_state.current_seq; + if (!_dataman_init) { + _dataman_init = true; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) { + _dataman_id = mission_state.dataman_id; + _count = mission_state.count; + _current_seq = mission_state.current_seq; - } else { - _dataman_id = 0; - _count = 0; - _current_seq = 0; - warnx("offboard mission init: ERROR"); + } else { + _dataman_id = 0; + _count = 0; + _current_seq = 0; + warnx("offboard mission init: ERROR"); + } } + _my_dataman_id = _dataman_id; } /** @@ -147,6 +157,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int _dataman_id = dataman_id; _count = count; _current_seq = seq; + _my_dataman_id = _dataman_id; /* mission state saved successfully, publish offboard_mission topic */ if (_offboard_mission_pub == nullptr) { @@ -159,8 +170,10 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int return OK; } else { - warnx("ERROR: can't save mission state"); - _mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state"); + warnx("WPM: ERROR: can't save mission state"); + if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { + _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); + } return ERROR; } @@ -243,7 +256,9 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext_critical("Unable to read from micro SD"); + if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { + _mavlink->send_statustext_critical("Mission storage: Unable to read from microSD"); + } if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } } @@ -407,7 +422,9 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) } else { _mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch"); - if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } + if (_verbose) { + warnx("WPM: MISSION_ACK ERR: ID mismatch"); + } } } } @@ -458,19 +475,17 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); - if (_count > 0) { - _state = MAVLINK_WPM_STATE_SENDLIST; - _transfer_seq = 0; - _transfer_count = _count; - _transfer_partner_sysid = msg->sysid; - _transfer_partner_compid = msg->compid; + _state = MAVLINK_WPM_STATE_SENDLIST; + _transfer_seq = 0; + _transfer_count = _count; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + if (_count > 0) { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } - - _mavlink->send_statustext_info("WPM: mission is empty"); } send_mission_count(msg->sysid, msg->compid, _count); @@ -564,11 +579,19 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); + + if(_transfer_in_progress) + { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + return; + } + _transfer_in_progress = true; if (wpc.count > _max_count) { if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE); + _transfer_in_progress = false; return; } @@ -577,9 +600,9 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* alternate dataman ID anyway to let navigator know about changes */ update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); - _mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION"); - // TODO send ACK? + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + _transfer_in_progress = false; return; } @@ -661,6 +684,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); _state = MAVLINK_WPM_STATE_IDLE; + _transfer_in_progress = false; return; } @@ -672,6 +696,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); _mavlink->send_statustext_critical("Unable to write on micro SD"); _state = MAVLINK_WPM_STATE_IDLE; + _transfer_in_progress = false; return; } @@ -688,8 +713,6 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) /* got all new mission items successfully */ if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } - _mavlink->send_statustext_info("WPM: Transfer complete."); - _state = MAVLINK_WPM_STATE_IDLE; if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { @@ -698,6 +721,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); } + _transfer_in_progress = false; } else { /* request next item */ @@ -754,24 +778,32 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->altitude_is_relative = true; break; - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; - case MAV_FRAME_MISSION: + // This is a mission item with no coordinate + break; + default: - return MAV_MISSION_ERROR; + return MAV_MISSION_UNSUPPORTED_FRAME; } switch (mavlink_mission_item->command) { case MAV_CMD_NAV_TAKEOFF: mission_item->pitch_min = mavlink_mission_item->param1; break; + case MAV_CMD_DO_JUMP: mission_item->do_jump_mission_index = mavlink_mission_item->param1; mission_item->do_jump_current_count = 0; mission_item->do_jump_repeat_count = mavlink_mission_item->param2; break; + + case MAV_CMD_DO_SET_SERVO: + + mission_item->actuator_num = mavlink_mission_item->param1; + mission_item->actuator_value = mavlink_mission_item->param2; + mission_item->time_inside=0.0f; + break; + default: mission_item->acceptance_radius = mavlink_mission_item->param2; mission_item->time_inside = mavlink_mission_item->param1; @@ -809,6 +841,11 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->param1 = mission_item->pitch_min; break; + case NAV_CMD_DO_SET_SERVO: + mavlink_mission_item->param1 = mission_item->actuator_num; + mavlink_mission_item->param2 = mission_item->actuator_value; + break; + case NAV_CMD_DO_JUMP: mavlink_mission_item->param1 = mission_item->do_jump_mission_index; mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count; @@ -832,3 +869,14 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * return OK; } + + +void MavlinkMissionManager::check_active_mission(void) +{ + if(!(_my_dataman_id==_dataman_id)) + { + if (_verbose) { warnx("WPM: New mission detected (possibly over different Mavlink instance) Updating"); } + _my_dataman_id=_dataman_id; + this->send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count); + } +} diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index a7bb94c22d14..c37322d37bba 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,9 +35,9 @@ * @file mavlink_mission.h * MAVLink mission manager interface definition. * - * @author Lorenz Meier - * @author Julian Oes - * @author Anton Babushkin + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin */ #pragma once @@ -97,6 +97,8 @@ class MavlinkMissionManager : public MavlinkStream { void set_verbose(bool v) { _verbose = v; } + void check_active_mission(void); + private: enum MAVLINK_WPM_STATES _state; ///< Current state @@ -106,10 +108,14 @@ class MavlinkMissionManager : public MavlinkStream { uint32_t _action_timeout; uint32_t _retry_timeout; unsigned _max_count; ///< Maximum number of mission items + unsigned _filesystem_errcount; ///< File system error count - int _dataman_id; ///< Dataman storage ID for active mission - unsigned _count; ///< Count of items in active mission - int _current_seq; ///< Current item sequence in active mission + static int _dataman_id; ///< Global Dataman storage ID for active mission + int _my_dataman_id; ///< class Dataman storage ID + static bool _dataman_init; ///< Dataman initialized + + static unsigned _count; ///< Count of items in active mission + static int _current_seq; ///< Current item sequence in active mission int _transfer_dataman_id; ///< Dataman storage ID for current transmission unsigned _transfer_count; ///< Items count in current transmission @@ -117,6 +123,7 @@ class MavlinkMissionManager : public MavlinkStream { unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission unsigned _transfer_partner_compid; ///< Partner component ID for current transmission + static bool _transfer_in_progress; ///< Global variable checking for current transmission int _offboard_mission_sub; int _mission_result_sub; @@ -126,6 +133,8 @@ class MavlinkMissionManager : public MavlinkStream { bool _verbose; + static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = 2; ///< Error count limit before stopping to report FS errors + /* do not allow top copying this class */ MavlinkMissionManager(MavlinkMissionManager &); MavlinkMissionManager& operator = (const MavlinkMissionManager &); diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index f8aa035298cf..368712e752cf 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,29 +36,34 @@ * Mavlink parameters manager implementation. * * @author Anton Babushkin + * @author Lorenz Meier */ #include +#include +#include + #include "mavlink_parameters.h" #include "mavlink_main.h" +ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s); +ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s); +#define HASH_PARAM "_HASH_CHECK" + MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), _send_all_index(-1), _rc_param_map_pub(nullptr), - _rc_param_map() + _rc_param_map(), + _uavcan_parameter_request_pub(nullptr), + _uavcan_parameter_value_sub(-1) { } unsigned MavlinkParametersManager::get_size() { - if (_send_all_index >= 0) { - return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - - } else { - return 0; - } + return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } void @@ -75,36 +80,75 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) _send_all_index = 0; } + + if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 && + (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + // publish list request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = req_list.target_component; + req.param_index = 0; + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); + } + } break; } case MAVLINK_MSG_ID_PARAM_SET: { /* set parameter */ - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - mavlink_param_set_t set; - mavlink_msg_param_set_decode(msg, &set); - - if (set.target_system == mavlink_system.sysid && - (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { - - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find_no_notification(name); - - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[pm] unknown param: %s", name); - _mavlink->send_statustext_info(buf); + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); - } else { - /* set and send parameter */ - param_set(param, &(set.param_value)); - send_param(param); - } + if (set.target_system == mavlink_system.sysid && + (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find_no_notification(name); + + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param: %s", name); + _mavlink->send_statustext_info(buf); + + } else { + /* set and send parameter */ + param_set(param, &(set.param_value)); + send_param(param); + } + } + + if (set.target_system == mavlink_system.sysid && set.target_component < 127 && + (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = set.target_component; + req.param_index = -1; + strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + if (set.param_type == MAV_PARAM_TYPE_REAL32) { + req.param_type = MAV_PARAM_TYPE_REAL32; + req.real_value = set.param_value; + } else { + int32_t val; + memcpy(&val, &set.param_value, sizeof(int32_t)); + req.param_type = MAV_PARAM_TYPE_INT64; + req.int_value = val; + } + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); } } break; @@ -120,17 +164,58 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* when no index is given, loop through string ids and compare them */ if (req_read.param_index < 0) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter and send it */ - send_param(param_find_no_notification(name)); + if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) { + /* return hash check for cached params */ + uint32_t hash = param_hash_check(); + + /* build the one-off response message */ + mavlink_param_value_t msg; + msg.param_count = param_count_used(); + msg.param_index = -1; + strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + msg.param_type = MAV_PARAM_TYPE_UINT32; + memcpy(&msg.param_value, &hash, sizeof(hash)); + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); + } else { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + send_param(param_find_no_notification(name)); + } } else { /* when index is >= 0, send this parameter again */ - send_param(param_for_used_index(req_read.param_index)); + int ret = send_param(param_for_used_index(req_read.param_index)); + + if (ret == 1) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + } else if (ret == 2) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + } + } + } + + if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 && + (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = req_read.target_component; + req.param_index = req_read.param_index; + strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); } } break; @@ -148,9 +233,9 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* Copy values from msg to uorb using the parameter_rc_channel_index as index */ size_t i = map_rc.parameter_rc_channel_index; _rc_param_map.param_index[i] = map_rc.param_index; - strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ - _rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0'; + _rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0'; _rc_param_map.scale[i] = map_rc.scale; _rc_param_map.value0[i] = map_rc.param_value0; _rc_param_map.value_min[i] = map_rc.param_value_min; @@ -181,11 +266,38 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) void MavlinkParametersManager::send(const hrt_abstime t) { - /* send all parameters if requested, but only after the system has booted */ - if (_send_all_index >= 0 && t > 4 * 1000 * 1000) { + bool space_available = _mavlink->get_free_tx_buf() >= get_size(); + + /* Send parameter values received from the UAVCAN topic */ + if (_uavcan_parameter_value_sub < 0) { + _uavcan_parameter_value_sub = orb_subscribe(ORB_ID(uavcan_parameter_value)); + } + + bool param_value_ready; + orb_check(_uavcan_parameter_value_sub, ¶m_value_ready); + if (space_available && param_value_ready) { + struct uavcan_parameter_value_s value; + orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value); + + mavlink_param_value_t msg; + msg.param_count = value.param_count; + msg.param_index = value.param_index; + strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + if (value.param_type == MAV_PARAM_TYPE_REAL32) { + msg.param_type = MAVLINK_TYPE_FLOAT; + msg.param_value = value.real_value; + } else { + int32_t val; + val = (int32_t)value.int_value; + memcpy(&msg.param_value, &val, sizeof(int32_t)); + msg.param_type = MAVLINK_TYPE_INT32_T; + } + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id); + } else if (_send_all_index >= 0 && _mavlink->boot_complete()) { + /* send all parameters if requested, but only after the system has booted */ /* skip if no space is available */ - if (_mavlink->get_free_tx_buf() < get_size()) { + if (!space_available) { return; } @@ -204,14 +316,18 @@ MavlinkParametersManager::send(const hrt_abstime t) if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) { _send_all_index = -1; } + } else if (_send_all_index == 0 && hrt_absolute_time() > 20 * 1000 * 1000) { + /* the boot did not seem to ever complete, warn user and set boot complete */ + _mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG."); + _mavlink->set_boot_complete(); } } -void +int MavlinkParametersManager::send_param(param_t param) { if (param == PARAM_INVALID) { - return; + return 1; } mavlink_param_value_t msg; @@ -221,7 +337,7 @@ MavlinkParametersManager::send_param(param_t param) * space during transmission, copy param onto float val_buf */ if (param_get(param, &msg.param_value) != OK) { - return; + return 2; } msg.param_count = param_count_used(); @@ -248,4 +364,6 @@ MavlinkParametersManager::send_param(param_t param) } _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); + + return 0; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index b6736f2128c7..d258f3b2405d 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +36,7 @@ * Mavlink parameters manager definition. * * @author Anton Babushkin + * @author Lorenz Meier */ #pragma once @@ -113,8 +114,11 @@ class MavlinkParametersManager : public MavlinkStream void send(const hrt_abstime t); - void send_param(param_t param); + int send_param(param_t param); orb_advert_t _rc_param_map_pub; struct rc_parameter_map_s _rc_param_map; + + orb_advert_t _uavcan_parameter_request_pub; + int _uavcan_parameter_value_sub; }; diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c new file mode 100644 index 000000000000..e50a95a25fad --- /dev/null +++ b/src/modules/mavlink/mavlink_params.c @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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. + * + ****************************************************************************/ + +/** + * MAVLink system ID + * @group MAVLink + * @min 1 + * @max 250 + */ +PARAM_DEFINE_INT32(MAV_SYS_ID, 1); + +/** + * MAVLink component ID + * @group MAVLink + * @min 1 + * @max 250 + */ +PARAM_DEFINE_INT32(MAV_COMP_ID, 1); + +/** + * MAVLink Radio ID + * + * When non-zero the MAVLink app will attempt to configure the + * radio to this ID and re-set the parameter to 0. If the value + * is negative it will reset the complete radio config to + * factory defaults. + * + * @group MAVLink + * @min -1 + * @max 240 + */ +PARAM_DEFINE_INT32(MAV_RADIO_ID, 0); + +/** + * MAVLink airframe type + * + * @min 1 + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_TYPE, 1); + +/** + * Use/Accept HIL GPS message even if not in HIL mode + * + * If set to 1 incoming HIL GPS messages are parsed. + * + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); + +/** + * Forward external setpoint messages + * + * If set to 1 incoming external setpoint messages will be directly forwarded + * to the controllers if in offboard control mode + * + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); + +/** + * Test parameter + * + * This parameter is not actively used by the system. Its purpose is to allow + * testing the parameter interface on the communication level. + * + * @group MAVLink + * @min -1000 + * @max 1000 + */ +PARAM_DEFINE_INT32(MAV_TEST_PAR, 1); + diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4e3da6c7c00d..0c2180a4d8b8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -35,9 +35,9 @@ * @file mavlink_receiver.cpp * MAVLink protocol message receive and dispatch * - * @author Lorenz Meier - * @author Anton Babushkin - * @author Thomas Gubler + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler */ /* XXX trim includes */ @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -62,7 +61,6 @@ #include #include #ifndef __PX4_POSIX -#include #include #endif #include @@ -118,7 +116,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _rates_sp_pub(nullptr), _force_sp_pub(nullptr), _pos_sp_triplet_pub(nullptr), - _vicon_position_pub(nullptr), + _att_pos_mocap_pub(nullptr), _vision_position_pub(nullptr), _telemetry_status_pub(nullptr), _rc_pub(nullptr), @@ -128,15 +126,20 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), + _hil_last_frame(0), _hil_local_proj_inited(0), _hil_local_alt0(0.0f), + _hil_prev_gyro{}, + _hil_prev_accel{}, _hil_local_proj_ref{}, _offboard_control_mode{}, _att_sp{}, _rates_sp{}, _time_offset_avg_alpha(0.6), _time_offset(0), - _orb_class_instance(-1) + _orb_class_instance(-1), + _mom_switch_pos{}, + _mom_switch_state(0) { } @@ -169,8 +172,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_set_mode(msg); break; - case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: - handle_message_vicon_position_estimate(msg); + case MAVLINK_MSG_ID_ATT_POS_MOCAP: + handle_message_att_pos_mocap(msg); break; case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: @@ -197,6 +200,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + handle_message_rc_channels_override(msg); + break; + case MAVLINK_MSG_ID_HEARTBEAT: handle_message_heartbeat(msg); break; @@ -275,7 +282,21 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) mavlink_command_long_t cmd_mavlink; mavlink_msg_command_long_decode(msg, &cmd_mavlink); - if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + /* evaluate if this system should accept this command */ + bool target_ok; + switch (cmd_mavlink.command) { + + case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: + /* broadcast */ + target_ok = (cmd_mavlink.target_system == 0); + break; + + default: + target_ok = (cmd_mavlink.target_system == mavlink_system.sysid); + break; + } + + if (target_ok && ((cmd_mavlink.target_component == mavlink_system.compid) || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { @@ -289,6 +310,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { /* send autopilot version message */ _mavlink->send_autopilot_capabilites(); + + } else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) { + _mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f); + } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { @@ -345,6 +370,13 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) /* terminate other threads and this thread */ _mavlink->_task_should_exit = true; + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { + /* send autopilot version message */ + _mavlink->send_autopilot_capabilites(); + + } else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) { + _mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f); + } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { @@ -556,27 +588,34 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) +MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) { - mavlink_vicon_position_estimate_t pos; - mavlink_msg_vicon_position_estimate_decode(msg, &pos); + mavlink_att_pos_mocap_t mocap; + mavlink_msg_att_pos_mocap_decode(msg, &mocap); + + struct att_pos_mocap_s att_pos_mocap; + memset(&att_pos_mocap, 0, sizeof(att_pos_mocap)); + + // Use the component ID to identify the mocap system + att_pos_mocap.id = msg->compid; - struct vehicle_vicon_position_s vicon_position; - memset(&vicon_position, 0, sizeof(vicon_position)); + att_pos_mocap.timestamp_boot = hrt_absolute_time(); // Monotonic time + att_pos_mocap.timestamp_computer = sync_stamp(mocap.time_usec); // Synced time - vicon_position.timestamp = hrt_absolute_time(); - vicon_position.x = pos.x; - vicon_position.y = pos.y; - vicon_position.z = pos.z; - vicon_position.roll = pos.roll; - vicon_position.pitch = pos.pitch; - vicon_position.yaw = pos.yaw; + att_pos_mocap.q[0] = mocap.q[0]; + att_pos_mocap.q[1] = mocap.q[1]; + att_pos_mocap.q[2] = mocap.q[2]; + att_pos_mocap.q[3] = mocap.q[3]; - if (_vicon_position_pub == nullptr) { - _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + att_pos_mocap.x = mocap.x; + att_pos_mocap.y = mocap.y; + att_pos_mocap.z = mocap.z; + + if (_att_pos_mocap_pub == nullptr) { + _att_pos_mocap_pub = orb_advertise(ORB_ID(att_pos_mocap), &att_pos_mocap); } else { - orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position); + orb_publish(ORB_ID(att_pos_mocap), _att_pos_mocap_pub, &att_pos_mocap); } } @@ -680,7 +719,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } /* set the yaw sp value */ - if (!offboard_control_mode.ignore_attitude) { + if (!offboard_control_mode.ignore_attitude && !isnan(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yaw_valid = true; pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; @@ -689,7 +728,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } /* set the yawrate sp value */ - if (!offboard_control_mode.ignore_bodyrate) { + if (!offboard_control_mode.ignore_bodyrate && !isnan(set_position_target_local_ned.yaw)) { pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; @@ -929,8 +968,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { - /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ - if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + /* telemetry status supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */ + if (_mavlink->get_channel() < ORB_MULTI_MAX_INSTANCES) { mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); @@ -939,7 +978,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.timestamp = hrt_absolute_time(); tstatus.telem_time = tstatus.timestamp; /* tstatus.heartbeat_time is set by system heartbeats */ - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; tstatus.txbuf = rstatus.txbuf; @@ -951,23 +990,96 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.component_id = msg->compid; if (_telemetry_status_pub == nullptr) { - _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); + int multi_instance; + _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH); } else { - orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); } } } -static switch_pos_t decode_switch_pos(uint16_t buttons, int sw) { - return (buttons >> (sw * 2)) & 3; +switch_pos_t +MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw) +{ + // This 2-bit method should be used in the future: (buttons >> (sw * 2)) & 3; + return (buttons & (1 << sw)) ? manual_control_setpoint_s::SWITCH_POS_ON : manual_control_setpoint_s::SWITCH_POS_OFF; } -static int decode_switch_pos_n(uint16_t buttons, int sw) { - if (buttons & (1 << sw)) { - return 1; +int +MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw) +{ + + bool on = (buttons & (1 << sw)); + + if (sw < MOM_SWITCH_COUNT) { + + bool last_on = (_mom_switch_state & (1 << sw)); + + /* first switch is 2-pos, rest is 2 pos */ + unsigned state_count = (sw == 0) ? 3 : 2; + + /* only transition on low state */ + if (!on && (on != last_on)) { + + _mom_switch_pos[sw]++; + if (_mom_switch_pos[sw] == state_count) { + _mom_switch_pos[sw] = 0; + } + } + + /* state_count - 1 is the number of intervals and 1000 is the range, + * with 2 states 0 becomes 0, 1 becomes 1000. With + * 3 states 0 becomes 0, 1 becomes 500, 2 becomes 1000, + * and so on for more states. + */ + return (_mom_switch_pos[sw] * 1000) / (state_count - 1) + 1000; + + } else { + /* return the current state */ + return on * 1000 + 1000; + } +} + +void +MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) +{ + mavlink_rc_channels_override_t man; + mavlink_msg_rc_channels_override_decode(msg, &man); + + // Check target + if (man.target_system != 0 && man.target_system != _mavlink->get_system_id()) { + return; + } + + struct rc_input_values rc = {}; + rc.timestamp_publication = hrt_absolute_time(); + rc.timestamp_last_signal = rc.timestamp_publication; + + rc.channel_count = 8; + rc.rc_failsafe = false; + rc.rc_lost = false; + rc.rc_lost_frame_count = 0; + rc.rc_total_frame_count = 1; + rc.rc_ppm_frame_length = 0; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; + rc.rssi = RC_INPUT_RSSI_MAX; + + /* channels */ + rc.values[0] = man.chan1_raw; + rc.values[1] = man.chan2_raw; + rc.values[2] = man.chan3_raw; + rc.values[3] = man.chan4_raw; + rc.values[4] = man.chan5_raw; + rc.values[5] = man.chan6_raw; + rc.values[6] = man.chan7_raw; + rc.values[7] = man.chan8_raw; + + if (_rc_pub == nullptr) { + _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); + } else { - return 0; + orb_publish(ORB_ID(input_rc), _rc_pub, &rc); } } @@ -994,35 +1106,30 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.rc_lost_frame_count = 0; rc.rc_total_frame_count = 1; rc.rc_ppm_frame_length = 0; - rc.input_source = RC_INPUT_SOURCE_MAVLINK; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; rc.rssi = RC_INPUT_RSSI_MAX; /* roll */ rc.values[0] = man.x / 2 + 1500; /* pitch */ rc.values[1] = man.y / 2 + 1500; + /* yaw */ + rc.values[2] = man.r / 2 + 1500; + /* throttle */ + rc.values[3] = fminf(fmaxf(man.z / 0.9f + 800, 1000.0f), 2000.0f); - /* - * yaw needs special handling as some joysticks have a circular mechanical mask, - * which makes the corner positions unreachable. - * scale yaw up and clip it to overcome this. - */ - rc.values[2] = man.r / 1.2f + 1500; - if (rc.values[2] > 2000) { - rc.values[2] = 2000; - } else if (rc.values[2] < 1000) { - rc.values[2] = 1000; + /* decode all switches which fit into the channel mask */ + unsigned max_switch = (sizeof(man.buttons) * 8); + unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0])); + if (max_switch > (max_channels - 4)) { + max_switch = (max_channels - 4); } - /* throttle */ - rc.values[3] = man.z + 1000; - - rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000; - rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000; - rc.values[6] = decode_switch_pos_n(man.buttons, 2) * 1000 + 1000; - rc.values[7] = decode_switch_pos_n(man.buttons, 3) * 1000 + 1000; - rc.values[8] = decode_switch_pos_n(man.buttons, 4) * 1000 + 1000; - rc.values[9] = decode_switch_pos_n(man.buttons, 5) * 1000 + 1000; + /* fill all channels */ + for (unsigned i = 0; i < max_switch; i++) { + rc.values[i + 4] = decode_switch_pos_n(man.buttons, i); + } + _mom_switch_state = man.buttons; if (_rc_pub == nullptr) { _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); @@ -1060,7 +1167,7 @@ void MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) { /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ - if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + if (_mavlink->get_channel() < ORB_MULTI_MAX_INSTANCES) { mavlink_heartbeat_t hb; mavlink_msg_heartbeat_decode(msg, &hb); @@ -1076,10 +1183,11 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) tstatus.heartbeat_time = tstatus.timestamp; if (_telemetry_status_pub == nullptr) { - _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); + int multi_instance; + _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH); } else { - orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); } } } @@ -1196,10 +1304,25 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) uint64_t timestamp = hrt_absolute_time(); + float dt; + + if (_hil_last_frame == 0 || + (timestamp <= _hil_last_frame) || + (timestamp - _hil_last_frame) > (0.1f * 1e6f) || + (_hil_last_frame >= timestamp)) { + dt = 0.01f; /* default to 100 Hz */ + memset(&_hil_prev_gyro[0], 0, sizeof(_hil_prev_gyro)); + _hil_prev_accel[0] = 0.0f; + _hil_prev_accel[1] = 0.0f; + _hil_prev_accel[2] = 9.866f; + } else { + dt = (timestamp - _hil_last_frame) / 1e6f; + } + _hil_last_frame = timestamp; + /* airspeed */ { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); + struct airspeed_s airspeed = {}; float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); // XXX need to fix this @@ -1219,8 +1342,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* gyro */ { - struct gyro_report gyro; - memset(&gyro, 0, sizeof(gyro)); + struct gyro_report gyro = {}; gyro.timestamp = timestamp; gyro.x_raw = imu.xgyro * 1000.0f; @@ -1241,8 +1363,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* accelerometer */ { - struct accel_report accel; - memset(&accel, 0, sizeof(accel)); + struct accel_report accel = {}; accel.timestamp = timestamp; accel.x_raw = imu.xacc / mg2ms2; @@ -1263,8 +1384,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* magnetometer */ { - struct mag_report mag; - memset(&mag, 0, sizeof(mag)); + struct mag_report mag = {}; mag.timestamp = timestamp; mag.x_raw = imu.xmag * 1000.0f; @@ -1285,8 +1405,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* baro */ { - struct baro_report baro; - memset(&baro, 0, sizeof(baro)); + struct baro_report baro = {}; baro.timestamp = timestamp; baro.pressure = imu.abs_pressure; @@ -1303,8 +1422,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* sensor combined */ { - struct sensor_combined_s hil_sensors; - memset(&hil_sensors, 0, sizeof(hil_sensors)); + struct sensor_combined_s hil_sensors = {}; hil_sensors.timestamp = timestamp; @@ -1314,6 +1432,13 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; + hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt; + hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt; + hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt; + memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_rad_s[0], sizeof(_hil_prev_gyro)); + hil_sensors.gyro_integral_dt[0] = dt * 1e6f; + hil_sensors.gyro_timestamp[0] = timestamp; + hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; @@ -1321,9 +1446,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[0] = imu.xacc; hil_sensors.accelerometer_m_s2[1] = imu.yacc; hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_timestamp = timestamp; + hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt; + hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt; + hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt; + memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_m_s2[0], sizeof(_hil_prev_accel)); + hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f; + hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16 + hil_sensors.accelerometer_timestamp[0] = timestamp; + hil_sensors.accelerometer_priority[0] = ORB_PRIO_HIGH; hil_sensors.adc_voltage_v[0] = 0.0f; hil_sensors.adc_voltage_v[1] = 0.0f; @@ -1335,18 +1466,20 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.magnetometer_ga[0] = imu.xmag; hil_sensors.magnetometer_ga[1] = imu.ymag; hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_timestamp = timestamp; + hil_sensors.magnetometer_range_ga[0] = 32.7f; // int16 + hil_sensors.magnetometer_mode[0] = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f; + hil_sensors.magnetometer_timestamp[0] = timestamp; + hil_sensors.magnetometer_priority[0] = ORB_PRIO_HIGH; - hil_sensors.baro_pres_mbar = imu.abs_pressure; - hil_sensors.baro_alt_meter = imu.pressure_alt; - hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_timestamp = timestamp; + hil_sensors.baro_pres_mbar[0] = imu.abs_pressure; + hil_sensors.baro_alt_meter[0] = imu.pressure_alt; + hil_sensors.baro_temp_celcius[0] = imu.temperature; + hil_sensors.baro_timestamp[0] = timestamp; - hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_timestamp = timestamp; + hil_sensors.differential_pressure_pa[0] = imu.diff_pressure * 1e2f; //from hPa to Pa + hil_sensors.differential_pressure_filtered_pa[0] = hil_sensors.differential_pressure_pa[0]; + hil_sensors.differential_pressure_timestamp[0] = timestamp; /* publish combined sensor topic */ if (_sensors_pub == nullptr) { @@ -1359,12 +1492,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* battery status */ { - struct battery_status_s hil_battery_status; - memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + struct battery_status_s hil_battery_status = {}; hil_battery_status.timestamp = timestamp; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.voltage_filtered_v = 11.1f; + hil_battery_status.voltage_v = 11.5f; + hil_battery_status.voltage_filtered_v = 11.5f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; @@ -1618,34 +1750,82 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) void * MavlinkReceiver::receive_thread(void *arg) { -#ifndef __PX4_POSIX - int uart_fd = _mavlink->get_uart_fd(); const int timeout = 500; - uint8_t buf[32]; - +#ifdef __PX4_POSIX + /* 1500 is the Wifi MTU, so we make sure to fit a full packet */ + uint8_t buf[1600]; +#else + /* the serial port buffers internally as well, we just need to fit a small chunk */ + uint8_t buf[64]; +#endif mavlink_message_t msg; - /* set thread name */ - char thread_name[24]; - sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); - prctl(PR_SET_NAME, thread_name, getpid()); - struct pollfd fds[1]; - fds[0].fd = uart_fd; - fds[0].events = POLLIN; + int uart_fd = -1; + + if (_mavlink->get_protocol() == SERIAL) { + uart_fd = _mavlink->get_uart_fd(); +#ifndef __PX4_POSIX + /* set thread name */ + char thread_name[24]; + sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); + prctl(PR_SET_NAME, thread_name, getpid()); +#endif + + fds[0].fd = uart_fd; + fds[0].events = POLLIN; + } +#ifdef __PX4_POSIX + struct sockaddr_in srcaddr; + socklen_t addrlen = sizeof(srcaddr); + + if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) { + // make sure mavlink app has booted before we start using the socket + while (!_mavlink->boot_complete()) { + usleep(100000); + } + + fds[0].fd = _mavlink->get_socket_fd(); + fds[0].events = POLLIN; + } + +#endif ssize_t nread = 0; while (!_mavlink->_task_should_exit) { - if (poll(fds, 1, timeout) > 0) { - - /* non-blocking read. read may return negative values */ - if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { - /* to avoid reading very small chunks wait for data before reading */ - usleep(1000); + if (poll(&fds[0], 1, timeout) > 0) { + if (_mavlink->get_protocol() == SERIAL) { + + /* + * to avoid reading very small chunks wait for data before reading + * this is designed to target one message, so >20 bytes at a time + */ + const unsigned character_count = 20; + + /* non-blocking read. read may return negative values */ + if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)character_count) { + unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000; + usleep(sleeptime); + } + } +#ifdef __PX4_POSIX + if (_mavlink->get_protocol() == UDP) { + if (fds[0].revents & POLLIN) { + nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen); + } + } else { + // could be TCP or other protocol } + struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address(); + int localhost = (127 << 24) + 1; + if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) { + // if we were sending to localhost before but have a new host then accept him + memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr)); + } +#endif /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { @@ -1661,7 +1841,6 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->count_rxbytes(nread); } } -#endif return NULL; } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8fffad4c3e57..2363516a9a35 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,8 +35,8 @@ * @file mavlink_orb_listener.h * MAVLink 1.0 uORB listener definition * - * @author Lorenz Meier - * @author Anton Babushkin + * @author Lorenz Meier + * @author Anton Babushkin */ #pragma once @@ -58,7 +58,7 @@ #include #include #include -#include +#include #include #include #include @@ -119,7 +119,7 @@ class MavlinkReceiver 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); + void handle_message_att_pos_mocap(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_set_position_target_local_ned(mavlink_message_t *msg); @@ -127,6 +127,7 @@ class MavlinkReceiver void handle_message_set_attitude_target(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_rc_channels_override(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_ping(mavlink_message_t *msg); void handle_message_request_data_stream(mavlink_message_t *msg); @@ -140,15 +141,26 @@ class MavlinkReceiver void *receive_thread(void *arg); /** - * Convert remote timestamp to local hrt time (usec) - * Use timesync if available, monotonic boot time otherwise - */ + * Convert remote timestamp to local hrt time (usec) + * Use timesync if available, monotonic boot time otherwise + */ uint64_t sync_stamp(uint64_t usec); + /** - * Exponential moving average filter to smooth time offset - */ + * Exponential moving average filter to smooth time offset + */ void smooth_time_offset(uint64_t offset_ns); + /** + * Decode a switch position from a bitfield + */ + switch_pos_t decode_switch_pos(uint16_t buttons, unsigned sw); + + /** + * Decode a switch position from a bitfield and state + */ + int decode_switch_pos_n(uint16_t buttons, unsigned sw); + mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; struct vehicle_land_detected_s hil_land_detector; @@ -174,7 +186,7 @@ class MavlinkReceiver orb_advert_t _rates_sp_pub; orb_advert_t _force_sp_pub; orb_advert_t _pos_sp_triplet_pub; - orb_advert_t _vicon_position_pub; + orb_advert_t _att_pos_mocap_pub; orb_advert_t _vision_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; @@ -184,8 +196,11 @@ class MavlinkReceiver int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; + uint64_t _hil_last_frame; bool _hil_local_proj_inited; float _hil_local_alt0; + float _hil_prev_gyro[3]; + float _hil_prev_accel[3]; struct map_projection_reference_s _hil_local_proj_ref; struct offboard_control_mode_s _offboard_control_mode; struct vehicle_attitude_setpoint_s _att_sp; @@ -194,6 +209,11 @@ class MavlinkReceiver uint64_t _time_offset; int _orb_class_instance; + static constexpr unsigned MOM_SWITCH_COUNT = 8; + + uint8_t _mom_switch_pos[MOM_SWITCH_COUNT]; + uint16_t _mom_switch_state; + /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver &); MavlinkReceiver operator=(const MavlinkReceiver &); diff --git a/src/modules/mavlink/mavlink_tests/CMakeLists.txt b/src/modules/mavlink/mavlink_tests/CMakeLists.txt new file mode 100644 index 000000000000..fe902974eec3 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/CMakeLists.txt @@ -0,0 +1,53 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__mavlink__mavlink_tests + MAIN mavlink_tests + STACK 5000 + COMPILE_FLAGS + -Weffc++ + -DMAVLINK_FTP_UNIT_TEST + -Wno-attributes + -Wno-packed + -Wno-packed + -Os + SRCS + mavlink_tests.cpp + mavlink_ftp_test.cpp + ../mavlink_stream.cpp + ../mavlink_ftp.cpp + ../mavlink.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt new file mode 100644 index 000000000000..6f857624c550 --- /dev/null +++ b/src/modules/mc_att_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if (${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=3500) +endif() +px4_add_module( + MODULE modules__mc_att_control + MAIN mc_att_control + STACK 1200 + COMPILE_FLAGS ${MODULE_CFLAGS} + SRCS + mc_att_control_main.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : 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 5b3a4a3503a6..477e364a5b36 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -39,7 +39,6 @@ * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011. * - * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin * @@ -75,7 +74,7 @@ #include #include #include -#include +#include #include #include #include @@ -100,6 +99,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f +#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f class MulticopterAttitudeControl { @@ -126,7 +126,7 @@ class MulticopterAttitudeControl bool _task_should_exit; /**< if true, task_main() should exit */ int _control_task; /**< task handle */ - int _v_att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ int _v_control_mode_sub; /**< vehicle control mode subscription */ @@ -136,7 +136,6 @@ class MulticopterAttitudeControl int _vehicle_status_sub; /**< vehicle status subscription */ int _motor_limits_sub; /**< motor limits subscription */ - orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ orb_advert_t _controller_status_pub; /**< controller status publication */ @@ -146,7 +145,7 @@ class MulticopterAttitudeControl bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ @@ -169,8 +168,6 @@ class MulticopterAttitudeControl math::Matrix<3, 3> _I; /**< identity matrix */ - bool _reset_yaw_sp; /**< reset yaw setpoint flag */ - struct { param_t roll_p; param_t roll_rate_p; @@ -192,12 +189,10 @@ class MulticopterAttitudeControl param_t pitch_rate_max; param_t yaw_rate_max; - param_t man_roll_max; - param_t man_pitch_max; - param_t man_yaw_max; param_t acro_roll_max; param_t acro_pitch_max; param_t acro_yaw_max; + param_t rattitude_thres; } _params_handles; /**< handles for interesting parameters */ @@ -214,11 +209,8 @@ class MulticopterAttitudeControl float yaw_rate_max; math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ - float man_roll_max; - float man_pitch_max; - float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - + float rattitude_thres; } _params; /** @@ -305,7 +297,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _control_task(-1), /* subscriptions */ - _v_att_sub(-1), + _ctrl_state_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), _params_sub(-1), @@ -314,7 +306,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _vehicle_status_sub(-1), /* publications */ - _att_sp_pub(nullptr), _v_rates_sp_pub(nullptr), _actuators_0_pub(nullptr), _controller_status_pub(nullptr), @@ -328,7 +319,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")) { - memset(&_v_att, 0, sizeof(_v_att)); + memset(&_ctrl_state, 0, sizeof(_ctrl_state)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); @@ -349,11 +340,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.roll_rate_max = 0.0f; _params.pitch_rate_max = 0.0f; _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; _params.mc_rate_max.zero(); _params.acro_rate_max.zero(); + _params.rattitude_thres = 1.0f; _rates_prev.zero(); _rates_sp.zero(); @@ -383,12 +372,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX"); _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); - _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); - _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); - _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); - _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + _params_handles.rattitude_thres = param_find("MC_RATT_TH"); /* fetch initial parameter values */ parameters_update(); @@ -469,14 +456,6 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max); - /* manual attitude control scale */ - param_get(_params_handles.man_roll_max, &_params.man_roll_max); - param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); - param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); - _params.man_roll_max = math::radians(_params.man_roll_max); - _params.man_pitch_max = math::radians(_params.man_pitch_max); - _params.man_yaw_max = math::radians(_params.man_yaw_max); - /* manual rate control scale and auto mode roll/pitch rate limits */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); @@ -485,6 +464,9 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); + /* stick deflection needed in rattitude mode to control rates not angles */ + param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); return OK; @@ -601,10 +583,10 @@ MulticopterAttitudeControl::vehicle_motor_limits_poll() } } -/* +/** * Attitude controller. - * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) - * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) + * Input: 'vehicle_attitude_setpoint' topics (depending on mode) + * Output: '_rates_sp' vector, '_thrust_sp' */ void MulticopterAttitudeControl::control_attitude(float dt) @@ -617,9 +599,9 @@ MulticopterAttitudeControl::control_attitude(float dt) math::Matrix<3, 3> R_sp; R_sp.set(_v_att_sp.R_body); - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.set(_v_att.R); + /* get current rotation matrix from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + math::Matrix<3, 3> R = q_att.to_dcm(); /* all input data is ready, run controller itself */ @@ -711,9 +693,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* current body angular rates */ math::Vector<3> rates; - rates(0) = _v_att.rollspeed; - rates(1) = _v_att.pitchspeed; - rates(2) = _v_att.yawspeed; + rates(0) = _ctrl_state.roll_rate; + rates(1) = _ctrl_state.pitch_rate; + rates(2) = _ctrl_state.yaw_rate; /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; @@ -751,7 +733,7 @@ MulticopterAttitudeControl::task_main() */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -765,7 +747,7 @@ MulticopterAttitudeControl::task_main() /* wakeup source: vehicle attitude */ px4_pollfd_struct_t fds[1]; - fds[0].fd = _v_att_sub; + fds[0].fd = _ctrl_state_sub; fds[0].events = POLLIN; while (!_task_should_exit) { @@ -801,8 +783,8 @@ MulticopterAttitudeControl::task_main() dt = 0.02f; } - /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + /* copy attitude and control state topics */ + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); /* check for updates in other topics */ parameter_update_poll(); @@ -812,29 +794,40 @@ MulticopterAttitudeControl::task_main() vehicle_status_poll(); vehicle_motor_limits_poll(); + /* Check if we are in rattitude mode and the pilot is above the threshold on pitch + * or roll (yaw can rotate 360 in normal att control). If both are true don't + * even bother running the attitude controllers */ + if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){ + if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || + fabsf(_manual_control_sp.x) > _params.rattitude_thres){ + _v_control_mode.flag_control_attitude_enabled = false; + } + } + if (_v_control_mode.flag_control_attitude_enabled) { - control_attitude(dt); - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); + control_attitude(dt); - if (_v_rates_sp_pub != nullptr) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); - } else if (_rates_sp_id) { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); - } + if (_v_rates_sp_pub != nullptr) { + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); + } else if (_rates_sp_id) { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } + //} } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); - _thrust_sp = _manual_control_sp.z; + _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); @@ -869,7 +862,7 @@ MulticopterAttitudeControl::task_main() _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _v_att.timestamp; + _actuators.timestamp_sample = _ctrl_state.timestamp; _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); @@ -979,4 +972,4 @@ int mc_att_control_main(int argc, char *argv[]) warnx("unrecognized command"); return 1; -} +} \ No newline at end of file diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index c9bf3753b459..0c7e4b0134cf 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -35,13 +35,10 @@ * @file mc_att_control_params.c * Parameters for multicopter attitude controller. * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin + * @author Lorenz Meier + * @author Anton Babushkin */ -#include - /** * Roll P gain * @@ -50,7 +47,7 @@ * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f); /** * Roll rate P gain @@ -60,7 +57,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f); /** * Roll rate I gain @@ -70,7 +67,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.05f); /** * Roll rate D gain @@ -80,7 +77,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f); /** * Roll rate feedforward @@ -101,7 +98,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); /** * Pitch rate P gain @@ -111,7 +108,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f); /** * Pitch rate I gain @@ -121,7 +118,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.05f); /** * Pitch rate D gain @@ -131,7 +128,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f); /** * Pitch rate feedforward @@ -152,7 +149,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.8f); /** * Yaw rate P gain @@ -162,7 +159,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f); /** * Yaw rate I gain @@ -172,7 +169,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f); /** * Yaw rate D gain @@ -215,7 +212,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 360.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 220.0f); /** * Max pitch rate @@ -227,19 +224,21 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 360.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 360.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f); /** * Max yaw rate * - * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * Limit for yaw rate, has effect for large rotations in autonomous mode, + * to avoid large control output and mixer saturation. A value of significantly + * over 60 degrees per second can already lead to mixer saturation. * * @unit deg/s * @min 0.0 * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f); /** * Max acro roll rate @@ -249,7 +248,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); +PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 360.0f); /** * Max acro pitch rate @@ -259,7 +258,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); +PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 360.0f); /** * Max acro yaw rate @@ -268,4 +267,17 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f); +PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f); + +/** + * Threshold for Rattitude mode + * + * Manual input needed in order to override attitude control rate setpoints + * and instead pass manual stick inputs as rate setpoints + * + * @unit + * @min 0.0 + * @max 1.0 + * @group Multicopter Attitude Control + */ + PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f); diff --git a/src/modules/mc_att_control_multiplatform/CMakeLists.txt b/src/modules/mc_att_control_multiplatform/CMakeLists.txt new file mode 100644 index 000000000000..60aefe210a9a --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__mc_att_control_multiplatform + MAIN mc_att_control_m + SRCS + mc_att_control_main.cpp + mc_att_control_start_nuttx.cpp + mc_att_control.cpp + mc_att_control_base.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index f5341155b8d8..98435a08f073 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -62,13 +62,11 @@ static const int ERROR = -1; } -MulticopterAttitudeControl::MulticopterAttitudeControl() : +MulticopterAttitudeControlMultiplatform::MulticopterAttitudeControlMultiplatform() : MulticopterAttitudeControlBase(), - _task_should_exit(false), _actuators_0_circuit_breaker_enabled(false), /* publications */ - _att_sp_pub(nullptr), _v_rates_sp_pub(nullptr), _actuators_0_pub(nullptr), _n(_appState), @@ -104,23 +102,23 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* * do subscriptions */ - _v_att = _n.subscribe(&MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); + _v_att = _n.subscribe(&MulticopterAttitudeControlMultiplatform::handle_vehicle_attitude, this, 0); _v_att_sp = _n.subscribe(0); _v_rates_sp = _n.subscribe(0); _v_control_mode = _n.subscribe(0); _parameter_update = _n.subscribe( - &MulticopterAttitudeControl::handle_parameter_update, this, 1000); + &MulticopterAttitudeControlMultiplatform::handle_parameter_update, this, 1000); _manual_control_sp = _n.subscribe(0); _armed = _n.subscribe(0); _v_status = _n.subscribe(0); } -MulticopterAttitudeControl::~MulticopterAttitudeControl() +MulticopterAttitudeControlMultiplatform::~MulticopterAttitudeControlMultiplatform() { } int -MulticopterAttitudeControl::parameters_update() +MulticopterAttitudeControlMultiplatform::parameters_update() { /* roll gains */ _params.att_p(0) = _params_handles.roll_p.update(); @@ -153,12 +151,12 @@ MulticopterAttitudeControl::parameters_update() return OK; } -void MulticopterAttitudeControl::handle_parameter_update(const px4_parameter_update &msg) +void MulticopterAttitudeControlMultiplatform::handle_parameter_update(const px4_parameter_update &msg) { parameters_update(); } -void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) { +void MulticopterAttitudeControlMultiplatform::handle_vehicle_attitude(const px4_vehicle_attitude &msg) { perf_begin(_loop_perf); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index aab41f97ba53..12d4beadfc9c 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -64,19 +64,19 @@ #include "mc_att_control_base.h" -class MulticopterAttitudeControl : +class MulticopterAttitudeControlMultiplatform : public MulticopterAttitudeControlBase { public: /** * Constructor */ - MulticopterAttitudeControl(); + MulticopterAttitudeControlMultiplatform(); /** * Destructor, also kills the sensors task. */ - ~MulticopterAttitudeControl(); + ~MulticopterAttitudeControlMultiplatform(); /* Callbacks for topics */ void handle_vehicle_attitude(const px4_vehicle_attitude &msg); @@ -85,10 +85,8 @@ class MulticopterAttitudeControl : void spin() { _n.spin(); } private: - bool _task_should_exit; /**< if true, sensor task should exit */ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - px4::Publisher *_att_sp_pub; /**< attitude setpoint publication */ px4::Publisher *_v_rates_sp_pub; /**< rate setpoint publication */ px4::Publisher *_actuators_0_pub; /**< attitude actuator controls publication */ diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp index 5a79a8c6b268..57e3416bd494 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp @@ -54,19 +54,24 @@ #include "mc_att_control.h" -bool thread_running = false; /**< Deamon status flag */ +bool mc_att_control_thread_running = false; /**< Deamon status flag */ +#if defined(__PX4_ROS) int main(int argc, char **argv) +#else +int mc_att_control_start_main(int argc, char **argv); // Prototype for missing declearation error with nuttx +int mc_att_control_start_main(int argc, char **argv) +#endif { px4::init(argc, argv, "mc_att_control_m"); PX4_INFO("starting"); - MulticopterAttitudeControl attctl; - thread_running = true; + MulticopterAttitudeControlMultiplatform attctl; + mc_att_control_thread_running = true; attctl.spin(); PX4_INFO("exiting."); - thread_running = false; + mc_att_control_thread_running = false; return 0; } diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c index 9c6ac5dc7340..e34732d1f3e0 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c @@ -36,14 +36,10 @@ * Parameters for multicopter attitude controller. * * @author Tobias Naegeli - * @author Lorenz Meier + * @author Lorenz Meier * @author Anton Babushkin */ -#include -#include "mc_att_control_params.h" -#include - /** * Roll P gain * diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h index c3b71715bb0e..bb7f7360e420 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h @@ -56,7 +56,7 @@ #define PARAM_MP_YAWRATE_I_DEFAULT 0.0f #define PARAM_MP_YAWRATE_D_DEFAULT 0.0f #define PARAM_MP_YAW_FF_DEFAULT 0.5f -#define PARAM_MP_YAWRATE_MAX_DEFAULT 120.0f +#define PARAM_MP_YAWRATE_MAX_DEFAULT 60.0f #define PARAM_MP_ACRO_R_MAX_DEFAULT 35.0f #define PARAM_MP_ACRO_P_MAX_DEFAULT 35.0f #define PARAM_MP_ACRO_Y_MAX_DEFAULT 120.0f diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index 93efd90e7f97..a0e6926cf43e 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -41,57 +41,58 @@ #include #include -extern bool thread_running; -int daemon_task; /**< Handle of deamon task / thread */ +extern bool mc_att_control_thread_running; +int mc_att_control_daemon_task; /**< Handle of deamon task / thread */ namespace px4 { -bool task_should_exit = false; +bool mc_att_control_task_should_exit = false; } using namespace px4; -extern int main(int argc, char **argv); +extern int mc_att_control_start_main(int argc, char **argv); extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]); int mc_att_control_m_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: mc_att_control_m {start|stop|status}"); + warnx("usage: mc_att_control_m {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { - if (thread_running) { + if (mc_att_control_thread_running) { warnx("already running"); /* this is not an error */ - exit(0); + return 0; } - task_should_exit = false; - - daemon_task = px4_task_spawn_cmd("mc_att_control_m", + mc_att_control_task_should_exit = false; + warnx("ok now btak running"); + mc_att_control_daemon_task = px4_task_spawn_cmd("mc_att_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1900, - main, + mc_att_control_start_main, (argv) ? (char* const*)&argv[2] : (char* const*)NULL); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { - task_should_exit = true; - exit(0); + mc_att_control_task_should_exit = true; + return 0; } if (!strcmp(argv[1], "status")) { - if (thread_running) { + if (mc_att_control_thread_running) { warnx("is running"); } else { warnx("not started"); } - exit(0); + return 0; } warnx("unrecognized command"); diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt new file mode 100644 index 000000000000..b501e657d8b3 --- /dev/null +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__mc_pos_control + MAIN mc_pos_control + STACK 1200 + SRCS + mc_pos_control_main.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : 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 766ede403c65..a26a65d4681f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include #include #include @@ -83,9 +83,13 @@ #include #include +#include +#include + #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f #define MIN_DIST 0.01f +#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f /** * Multicopter position control app start / stop handling function @@ -94,7 +98,7 @@ */ extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]); -class MulticopterPositionControl +class MulticopterPositionControl : public control::SuperBlock { public: /** @@ -115,20 +119,21 @@ class MulticopterPositionControl int start(); private: - const float alt_ctl_dz = 0.2f; + const float alt_ctl_dz = 0.1f; bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ int _mavlink_fd; /**< mavlink fd */ - int _att_sub; /**< vehicle attitude subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _pos_sp_triplet_sub; /**< position setpoint triplet */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ int _local_pos_sp_sub; /**< offboard local position setpoint */ int _global_vel_sp_sub; /**< offboard global velocity setpoint */ @@ -136,16 +141,23 @@ class MulticopterPositionControl orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ + struct control_state_s _ctrl_state; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ - struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + + control::BlockParamFloat _manual_thr_min; + control::BlockParamFloat _manual_thr_max; + control::BlockDerivative _vel_x_deriv; + control::BlockDerivative _vel_y_deriv; + control::BlockDerivative _vel_z_deriv; struct { param_t thr_min; @@ -169,6 +181,10 @@ class MulticopterPositionControl param_t man_pitch_max; param_t man_yaw_max; param_t mc_att_yaw_p; + param_t hold_xy_dz; + param_t hold_z_dz; + param_t hold_max_xy; + param_t hold_max_z; } _params_handles; /**< handles for interesting parameters */ struct { @@ -181,6 +197,10 @@ class MulticopterPositionControl float man_pitch_max; float man_yaw_max; float mc_att_yaw_p; + float hold_xy_dz; + float hold_z_dz; + float hold_max_xy; + float hold_max_z; math::Vector<3> pos_p; math::Vector<3> vel_p; @@ -198,6 +218,10 @@ class MulticopterPositionControl bool _reset_pos_sp; bool _reset_alt_sp; bool _mode_auto; + bool _pos_hold_engaged; + bool _alt_hold_engaged; + bool _run_pos_control; + bool _run_alt_control; math::Vector<3> _pos; math::Vector<3> _pos_sp; @@ -205,12 +229,14 @@ class MulticopterPositionControl math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ math::Vector<3> _vel_ff; - math::Vector<3> _sp_move_rate; + + math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ + float _yaw; /**< yaw angle (euler) */ /** * Update our local parameter cache. */ - int parameters_update(bool force); + int parameters_update(bool force); /** * Update control outputs @@ -253,8 +279,8 @@ class MulticopterPositionControl */ void control_offboard(float dt); - bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, - const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + bool cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res); /** * Set position setpoint for AUTO @@ -290,13 +316,13 @@ MulticopterPositionControl *g_control; } MulticopterPositionControl::MulticopterPositionControl() : - + SuperBlock(NULL, "MPC"), _task_should_exit(false), _control_task(-1), _mavlink_fd(-1), -/* subscriptions */ - _att_sub(-1), + /* subscriptions */ + _ctrl_state_sub(-1), _att_sp_sub(-1), _control_mode_sub(-1), _params_sub(-1), @@ -306,19 +332,29 @@ MulticopterPositionControl::MulticopterPositionControl() : _pos_sp_triplet_sub(-1), _global_vel_sp_sub(-1), -/* publications */ + /* publications */ _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), - + _manual_thr_min(this, "MANTHR_MIN"), + _manual_thr_max(this, "MANTHR_MAX"), + _vel_x_deriv(this, "VELD"), + _vel_y_deriv(this, "VELD"), + _vel_z_deriv(this, "VELD"), _ref_alt(0.0f), _ref_timestamp(0), _reset_pos_sp(true), _reset_alt_sp(true), - _mode_auto(false) + _mode_auto(false), + _pos_hold_engaged(false), + _alt_hold_engaged(false), + _run_pos_control(true), + _run_alt_control(true), + _yaw(0.0f) { - memset(&_att, 0, sizeof(_att)); + memset(&_vehicle_status, 0, sizeof(_vehicle_status)); + memset(&_ctrl_state, 0, sizeof(_ctrl_state)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); @@ -344,7 +380,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_sp.zero(); _vel_prev.zero(); _vel_ff.zero(); - _sp_move_rate.zero(); + + _R.identity(); _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); @@ -367,6 +404,11 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX"); _params_handles.mc_att_yaw_p = param_find("MC_YAW_P"); + _params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ"); + _params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ"); + _params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY"); + _params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z"); + /* fetch initial parameter values */ parameters_update(true); @@ -409,6 +451,10 @@ MulticopterPositionControl::parameters_update(bool force) } if (updated || force) { + /* update C++ param system */ + updateParams(); + + /* update legacy C interface params */ param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); @@ -450,6 +496,16 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.z_ff, &v); v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; + param_get(_params_handles.hold_xy_dz, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.hold_xy_dz = v; + param_get(_params_handles.hold_z_dz, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.hold_z_dz = v; + param_get(_params_handles.hold_max_xy, &v); + _params.hold_max_xy = (v < 0.0f ? 0.0f : v); + param_get(_params_handles.hold_max_z, &v); + _params.hold_max_z = (v < 0.0f ? 0.0f : v); _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; @@ -461,7 +517,7 @@ MulticopterPositionControl::parameters_update(bool force) _params.man_roll_max = math::radians(_params.man_roll_max); _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); - param_get(_params_handles.mc_att_yaw_p,&v); + param_get(_params_handles.mc_att_yaw_p, &v); _params.mc_att_yaw_p = v; } @@ -473,10 +529,16 @@ MulticopterPositionControl::poll_subscriptions() { bool updated; - orb_check(_att_sub, &updated); + orb_check(_vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } + + orb_check(_ctrl_state_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); } orb_check(_att_sp_sub, &updated); @@ -564,9 +626,9 @@ MulticopterPositionControl::reset_pos_sp() _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + - _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_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); + - _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -576,7 +638,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); + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _vel_sp(2)) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } @@ -607,31 +669,20 @@ MulticopterPositionControl::limit_pos_sp_offset() void MulticopterPositionControl::control_manual(float dt) { - _sp_move_rate.zero(); + math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range + req_vel_sp.zero(); if (_control_mode.flag_control_altitude_enabled) { - /* move altitude setpoint with throttle stick */ - _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); + /* set vertical velocity setpoint with throttle stick */ + req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); // D } if (_control_mode.flag_control_position_enabled) { - /* move position setpoint with roll/pitch stick */ - _sp_move_rate(0) = _manual.x; - _sp_move_rate(1) = _manual.y; + /* set horizontal velocity setpoint with roll/pitch stick */ + req_vel_sp(0) = _manual.x; + req_vel_sp(1) = _manual.y; } - /* limit setpoint move rate */ - float sp_move_norm = _sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - _sp_move_rate /= sp_move_norm; - } - - /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); - if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); @@ -642,30 +693,75 @@ MulticopterPositionControl::control_manual(float dt) reset_pos_sp(); } - /* feed forward setpoint move rate with weight vel_ff */ - _vel_ff = _sp_move_rate.emult(_params.vel_ff); + /* limit velocity setpoint */ + float req_vel_sp_norm = req_vel_sp.length(); + + if (req_vel_sp_norm > 1.0f) { + req_vel_sp /= req_vel_sp_norm; + } - /* move position setpoint */ - _pos_sp += _sp_move_rate * dt; + /* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult( + _params.vel_max); // in NED and scaled to actual velocity - /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs; - pos_sp_offs.zero(); + /* + * assisted velocity mode: user controls velocity, but if velocity is small enough, position + * hold is activated for the corresponding axis + */ + /* horizontal axes */ if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + /* check for pos. hold */ + if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) { + if (!_pos_hold_engaged) { + if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy + && fabsf(_vel(1)) < _params.hold_max_xy)) { + _pos_hold_engaged = true; + + } else { + _pos_hold_engaged = false; + } + } + + } else { + _pos_hold_engaged = false; + } + + /* set requested velocity setpoint */ + if (!_pos_hold_engaged) { + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ + _vel_sp(0) = req_vel_sp_scaled(0); + _vel_sp(1) = req_vel_sp_scaled(1); + } } + /* vertical axis */ if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); - } + /* check for pos. hold */ + if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) { + if (!_alt_hold_engaged) { + if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) { + _alt_hold_engaged = true; - float pos_sp_offs_norm = pos_sp_offs.length(); + } else { + _alt_hold_engaged = false; + } + } - if (pos_sp_offs_norm > 1.0f) { - pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } else { + _alt_hold_engaged = false; + } + + /* set requested velocity setpoint */ + if (!_alt_hold_engaged) { + _run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */ + _vel_sp(2) = req_vel_sp_scaled(2); + _pos_sp(2) = _pos(2); + } } } @@ -684,18 +780,22 @@ MulticopterPositionControl::control_offboard(float dt) /* control position */ _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ /* reset position setpoint to current position if needed */ reset_pos_sp(); /* set position setpoint move rate */ - _sp_move_rate(0) = _pos_sp_triplet.current.vx; - _sp_move_rate(1) = _pos_sp_triplet.current.vy; + _vel_sp(0) = _pos_sp_triplet.current.vx; + _vel_sp(1) = _pos_sp_triplet.current.vy; + + _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ } if (_pos_sp_triplet.current.yaw_valid) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } else if (_pos_sp_triplet.current.yawspeed_valid) { _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } @@ -703,19 +803,16 @@ MulticopterPositionControl::control_offboard(float dt) if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) { /* Control altitude */ _pos_sp(2) = _pos_sp_triplet.current.z; + } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); /* set altitude setpoint move rate */ - _sp_move_rate(2) = _pos_sp_triplet.current.vz; - } - - /* feed forward setpoint move rate with weight vel_ff */ - _vel_ff = _sp_move_rate.emult(_params.vel_ff); + _vel_sp(2) = _pos_sp_triplet.current.vz; - /* move position setpoint */ - _pos_sp += _sp_move_rate * dt; + _run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */ + } } else { reset_pos_sp(); @@ -724,8 +821,8 @@ MulticopterPositionControl::control_offboard(float dt) } bool -MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, - const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res) { /* project center of sphere on line */ /* normalized AB */ @@ -760,30 +857,58 @@ void MulticopterPositionControl::control_auto(float dt) //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 (!PX4_ISFINITE(_pos_sp_triplet.current.lat) || - !PX4_ISFINITE(_pos_sp_triplet.current.lon) || - !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { + !PX4_ISFINITE(_pos_sp_triplet.current.lon) || + !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { _pos_sp_triplet.current.valid = false; } } + bool current_setpoint_valid = false; + bool previous_setpoint_valid = false; + + math::Vector<3> prev_sp; + math::Vector<3> curr_sp; + if (_pos_sp_triplet.current.valid) { - /* in case of interrupted mission don't go to waypoint but stay at current position */ - _reset_pos_sp = true; - _reset_alt_sp = true; /* project setpoint to local frame */ - math::Vector<3> curr_sp; map_projection_project(&_ref_pos, _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, &curr_sp.data[0], &curr_sp.data[1]); curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); - /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + if (PX4_ISFINITE(curr_sp(0)) && + PX4_ISFINITE(curr_sp(1)) && + PX4_ISFINITE(curr_sp(2))) { + current_setpoint_valid = true; + } + } + + if (_pos_sp_triplet.previous.valid) { + map_projection_project(&_ref_pos, + _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); + prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); + + if (PX4_ISFINITE(prev_sp(0)) && + PX4_ISFINITE(prev_sp(1)) && + PX4_ISFINITE(prev_sp(2))) { + previous_setpoint_valid = true; + } + } + + if (current_setpoint_valid) { + /* in case of interrupted mission don't go to waypoint but stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; + + /* scaled space: 1 == position error resulting max allowed speed */ math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here /* convert current setpoint to scaled space */ @@ -792,30 +917,26 @@ void MulticopterPositionControl::control_auto(float dt) /* by default use current setpoint as is */ math::Vector<3> pos_sp_s = curr_sp_s; - if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && previous_setpoint_valid) { /* follow "previous - current" line */ - math::Vector<3> prev_sp; - map_projection_project(&_ref_pos, - _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, - &prev_sp.data[0], &prev_sp.data[1]); - prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); if ((curr_sp - prev_sp).length() > MIN_DIST) { - /* find X - cross point of L1 sphere and trajectory */ + /* find X - cross point of unit sphere and trajectory */ math::Vector<3> pos_s = _pos.emult(scale); math::Vector<3> prev_sp_s = prev_sp.emult(scale); math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; math::Vector<3> curr_pos_s = pos_s - curr_sp_s; float curr_pos_s_len = curr_pos_s.length(); + if (curr_pos_s_len < 1.0f) { - /* copter is closer to waypoint than L1 radius */ + /* copter is closer to waypoint than unit radius */ /* check next waypoint and use it to avoid slowing down when passing via waypoint */ if (_pos_sp_triplet.next.valid) { math::Vector<3> next_sp; map_projection_project(&_ref_pos, - _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, - &next_sp.data[0], &next_sp.data[1]); + _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, + &next_sp.data[0], &next_sp.data[1]); next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt); if ((next_sp - curr_sp).length() > MIN_DIST) { @@ -833,15 +954,16 @@ void MulticopterPositionControl::control_auto(float dt) if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { float curr_next_s_len = curr_next_s.length(); - /* if curr - next distance is larger than L1 radius, limit it */ + + /* if curr - next distance is larger than unit radius, limit it */ if (curr_next_s_len > 1.0f) { cos_a_curr_next /= curr_next_s_len; } /* feed forward position setpoint offset */ math::Vector<3> pos_ff = prev_curr_s_norm * - cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * - (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); pos_sp_s += pos_ff; } } @@ -849,8 +971,9 @@ void MulticopterPositionControl::control_auto(float dt) } else { bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); + if (near) { - /* L1 sphere crosses trajectory */ + /* unit sphere crosses trajectory */ } else { /* copter is too far from trajectory */ @@ -876,6 +999,7 @@ void MulticopterPositionControl::control_auto(float dt) /* difference between current and desired position setpoints, 1 = max speed */ math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); } @@ -897,12 +1021,13 @@ void MulticopterPositionControl::task_main() { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); /* * do subscriptions */ - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -957,12 +1082,23 @@ MulticopterPositionControl::task_main() } poll_subscriptions(); + + /* get current rotation matrix and euler angles from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R = q_att.to_dcm(); + math::Vector<3> euler_angles; + euler_angles = _R.to_euler(); + _yaw = euler_angles(2); + parameters_update(false); hrt_abstime t = hrt_absolute_time(); float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; t_prev = t; + // set dt for control blocks + setDt(dt); + if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ _reset_pos_sp = true; @@ -972,6 +1108,14 @@ MulticopterPositionControl::task_main() reset_yaw_sp = true; } + /* reset yaw and altitude setpoint for VTOL which are in fw mode */ + if (_vehicle_status.is_vtol) { + if (!_vehicle_status.is_rotary_wing) { + reset_yaw_sp = true; + _reset_alt_sp = true; + } + } + //Update previous arming state was_armed = _control_mode.flag_armed; @@ -991,7 +1135,11 @@ MulticopterPositionControl::task_main() _vel(2) = _local_pos.vz; _vel_ff.zero(); - _sp_move_rate.zero(); + + /* by default, run position/altitude controller. the control_* functions + * can disable this and run velocity controllers directly in this cycle */ + _run_pos_control = true; + _run_alt_control = true; /* select control source */ if (_control_mode.flag_control_manual_enabled) { @@ -1009,7 +1157,8 @@ MulticopterPositionControl::task_main() control_auto(dt); } - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); @@ -1017,7 +1166,7 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = _att.yaw; + _att_sp.yaw_body = _yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); @@ -1031,24 +1180,53 @@ MulticopterPositionControl::task_main() } } else { - /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err = _pos_sp - _pos; + /* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */ + if (_run_pos_control) { + _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0); + _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1); + } - _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + if (_run_alt_control) { + _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2); + } - if (!_control_mode.flag_control_altitude_enabled) { - _reset_alt_sp = true; - _vel_sp(2) = 0.0f; + /* make sure velocity setpoint is saturated in xy*/ + float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + + _vel_sp(1) * _vel_sp(1)); + + if (vel_norm_xy > _params.vel_max(0)) { + /* note assumes vel_max(0) == vel_max(1) */ + _vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy; + _vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy; + } + + /* make sure velocity setpoint is saturated in z*/ + float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2)); + + if (vel_norm_z > _params.vel_max(2)) { + _vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z; } if (!_control_mode.flag_control_position_enabled) { _reset_pos_sp = true; + } + + if (!_control_mode.flag_control_altitude_enabled) { + _reset_alt_sp = true; + } + + if (!_control_mode.flag_control_velocity_enabled) { _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } + if (!_control_mode.flag_control_climb_rate_enabled) { + _vel_sp(2) = 0.0f; + } + /* use constant descend rate when landing, ignore altitude setpoint */ - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -1103,8 +1281,12 @@ MulticopterPositionControl::task_main() /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; - /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + /* derivative of velocity error, / + * does not includes setpoint acceleration */ + math::Vector<3> vel_err_d; + vel_err_d(0) = _vel_x_deriv.update(-_vel(0)); + vel_err_d(1) = _vel_y_deriv.update(-_vel(1)); + vel_err_d(2) = _vel_z_deriv.update(-_vel(2)); /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; @@ -1134,7 +1316,7 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; @@ -1172,11 +1354,11 @@ MulticopterPositionControl::task_main() /* thrust compensation for altitude only control mode */ float att_comp; - if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) { - att_comp = 1.0f / PX4_R(_att.R, 2, 2); + if (_R(2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / _R(2, 2); - } else if (PX4_R(_att.R, 2, 2) > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f; + } else if (_R(2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f; saturation_z = true; } else { @@ -1344,6 +1526,7 @@ MulticopterPositionControl::task_main() /* publish local position setpoint */ if (_local_pos_sp_pub != nullptr) { orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + } else { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); } @@ -1357,45 +1540,49 @@ MulticopterPositionControl::task_main() reset_int_xy = true; } - // generate attitude setpoint from manual controls - if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { + /* generate attitude setpoint from manual controls */ + if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { - // reset yaw setpoint to current position if needed + /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; + _att_sp.yaw_body = _yaw; } - // do not move yaw while arming - else if (_manual.z > 0.1f) - { - const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p; + /* do not move yaw while arming */ + else if (_manual.z > 0.1f) { + const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p; _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); - float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw); - if (yaw_offs < - YAW_OFFSET_MAX) { - _att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX); + float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); + float yaw_offs = _wrap_pi(yaw_target - _yaw); - } else if (yaw_offs > YAW_OFFSET_MAX) { - _att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); + // If the yaw offset became too big for the system to track stop + // shifting it + if (fabsf(yaw_offs) < yaw_offset_max) { + _att_sp.yaw_body = yaw_target; } } - //Control roll and pitch directly if we no aiding velocity controller is active - if(!_control_mode.flag_control_velocity_enabled) { + /* control roll and pitch directly if we no aiding velocity controller is active */ + if (!_control_mode.flag_control_velocity_enabled) { _att_sp.roll_body = _manual.y * _params.man_roll_max; _att_sp.pitch_body = -_manual.x * _params.man_pitch_max; } - //Control climb rate directly if no aiding altitude controller is active - if(!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = _manual.z; + /* control throttle directly if no climb rate controller is active */ + if (!_control_mode.flag_control_climb_rate_enabled) { + _att_sp.thrust = math::min(_manual.z, _manual_thr_max.get()); + + /* enforce minimum throttle if not landed */ + if (!_vehicle_status.condition_landed) { + _att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get()); + } } - //Construct attitude setpoint rotation matrix - math::Matrix<3,3> R_sp; - R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); + /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); /* copy quaternion setpoint to attitude setpoint topic */ @@ -1403,8 +1590,8 @@ MulticopterPositionControl::task_main() q_sp.from_dcm(R_sp); memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.timestamp = hrt_absolute_time(); - } - else { + + } else { reset_yaw_sp = true; } @@ -1416,18 +1603,19 @@ MulticopterPositionControl::task_main() * in this case the attitude setpoint is published by the mavlink app */ if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled))) { - if (_att_sp_pub != nullptr) { + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { + if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - } else { + } else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ - reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled; + reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled + && !_control_mode.flag_control_climb_rate_enabled; } mavlink_log_info(_mavlink_fd, "[mpc] stopped"); @@ -1442,11 +1630,11 @@ MulticopterPositionControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("mc_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - (px4_main_t)&MulticopterPositionControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (px4_main_t)&MulticopterPositionControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index fc73b31f59fa..7c63a82afa27 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin + * 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 @@ -36,13 +35,36 @@ * @file mc_pos_control_params.c * Multicopter position controller parameters. * - * @author Anton Babushkin + * @author Anton Babushkin */ -#include +/** + * Minimum thrust in auto thrust control + * + * It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.05 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); /** - * Minimum thrust + * Maximum thrust in auto thrust control + * + * Limit max allowed thrust. Setting a value of one can put + * the system into actuator saturation as no spread between + * the motors is possible any more. A value of 0.8 - 0.9 + * is recommended. + * + * @min 0.0 + * @max 0.95 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f); + +/** + * Minimum manual thrust * * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. * @@ -50,18 +72,21 @@ * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f); +PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.12f); /** - * Maximum thrust + * Maximum manual thrust * - * Limit max allowed thrust. + * Limit max allowed thrust. Setting a value of one can put + * the system into actuator saturation as no spread between + * the motors is possible any more. A value of 0.8 - 0.9 + * is recommended. * * @min 0.0 * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_MANTHR_MAX, 0.9f); /** * Proportional gain for vertical position error @@ -77,7 +102,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); * @min 0.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f); /** * Integral gain for vertical velocity error @@ -104,9 +129,10 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); * * @unit m/s * @min 0.0 + * @max 8.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); /** * Vertical velocity feed forward @@ -180,7 +206,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); * * Limits maximum tilt in AUTO and POSCTRL modes during flight. * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -192,7 +218,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); * * Limits maximum tilt angle on landing. * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -211,7 +237,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); /** * Max manual roll * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -221,7 +247,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX, 35.0f); /** * Max manual pitch * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -231,9 +257,55 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f); /** * Max manual yaw rate * - * @unit deg/s + * @unit degree / s * @min 0.0 * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f); +/** + * Deadzone of X,Y sticks where position hold is enabled + * + * @unit % + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_XY_DZ, 0.1f); + +/** + * Deadzone of Z stick where altitude hold is enabled + * + * @unit % + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_Z_DZ, 0.1f); + +/** + * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.5f); + +/** + * Maximum vertical velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f); + +/** + * Low pass filter cut freq. for numerical velocity derivative + * + * @unit Hz + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); diff --git a/src/modules/mc_pos_control_multiplatform/CMakeLists.txt b/src/modules/mc_pos_control_multiplatform/CMakeLists.txt new file mode 100644 index 000000000000..f36acf4e3342 --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__mc_pos_control_multiplatform + MAIN mc_pos_control_m + SRCS + mc_pos_control_main.cpp + mc_pos_control_start_nuttx.cpp + mc_pos_control.cpp + mc_pos_control_params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index c14a4b76273f..e9bc2346ccb6 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -49,7 +49,7 @@ #define SIGMA 0.000001f #define MIN_DIST 0.01f -MulticopterPositionControl::MulticopterPositionControl() : +MulticopterPositionControlMultiplatform::MulticopterPositionControlMultiplatform() : _task_should_exit(false), _control_task(-1), @@ -105,14 +105,14 @@ MulticopterPositionControl::MulticopterPositionControl() : /* * Do subscriptions */ - _att = _n.subscribe(&MulticopterPositionControl::handle_vehicle_attitude, this, 0); + _att = _n.subscribe(&MulticopterPositionControlMultiplatform::handle_vehicle_attitude, this, 0); _control_mode = _n.subscribe(0); _parameter_update = _n.subscribe( - &MulticopterPositionControl::handle_parameter_update, this, 1000); + &MulticopterPositionControlMultiplatform::handle_parameter_update, this, 1000); _manual_control_sp = _n.subscribe(0); _armed = _n.subscribe(0); _local_pos = _n.subscribe(0); - _pos_sp_triplet = _n.subscribe(&MulticopterPositionControl::handle_position_setpoint_triplet, this, 0); + _pos_sp_triplet = _n.subscribe(&MulticopterPositionControlMultiplatform::handle_position_setpoint_triplet, this, 0); _local_pos_sp = _n.subscribe(0); _global_vel_sp = _n.subscribe(0); @@ -139,12 +139,12 @@ MulticopterPositionControl::MulticopterPositionControl() : _R.identity(); } -MulticopterPositionControl::~MulticopterPositionControl() +MulticopterPositionControlMultiplatform::~MulticopterPositionControlMultiplatform() { } int -MulticopterPositionControl::parameters_update() +MulticopterPositionControlMultiplatform::parameters_update() { _params.thr_min = _params_handles.thr_min.update(); _params.thr_max = _params_handles.thr_max.update(); @@ -180,7 +180,7 @@ MulticopterPositionControl::parameters_update() float -MulticopterPositionControl::scale_control(float ctl, float end, float dz) +MulticopterPositionControlMultiplatform::scale_control(float ctl, float end, float dz) { if (ctl > dz) { return (ctl - dz) / (end - dz); @@ -194,7 +194,7 @@ MulticopterPositionControl::scale_control(float ctl, float end, float dz) } void -MulticopterPositionControl::update_ref() +MulticopterPositionControlMultiplatform::update_ref() { if (_local_pos->data().ref_timestamp != _ref_timestamp) { double lat_sp, lon_sp; @@ -221,7 +221,7 @@ MulticopterPositionControl::update_ref() } void -MulticopterPositionControl::reset_pos_sp() +MulticopterPositionControlMultiplatform::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; @@ -238,7 +238,7 @@ MulticopterPositionControl::reset_pos_sp() } void -MulticopterPositionControl::reset_alt_sp() +MulticopterPositionControlMultiplatform::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; @@ -255,7 +255,7 @@ MulticopterPositionControl::reset_alt_sp() } void -MulticopterPositionControl::limit_pos_sp_offset() +MulticopterPositionControlMultiplatform::limit_pos_sp_offset() { math::Vector<3> pos_sp_offs; pos_sp_offs.zero(); @@ -278,7 +278,7 @@ MulticopterPositionControl::limit_pos_sp_offset() } void -MulticopterPositionControl::control_manual(float dt) +MulticopterPositionControlMultiplatform::control_manual(float dt) { _sp_move_rate.zero(); @@ -343,7 +343,7 @@ MulticopterPositionControl::control_manual(float dt) } void -MulticopterPositionControl::control_offboard(float dt) +MulticopterPositionControlMultiplatform::control_offboard(float dt) { if (_pos_sp_triplet->data().current.valid) { if (_control_mode->data().flag_control_position_enabled && _pos_sp_triplet->data().current.position_valid) { @@ -390,7 +390,7 @@ MulticopterPositionControl::control_offboard(float dt) } bool -MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, +MulticopterPositionControlMultiplatform::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) { /* project center of sphere on line */ @@ -415,7 +415,7 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f } void -MulticopterPositionControl::control_auto(float dt) +MulticopterPositionControlMultiplatform::control_auto(float dt) { if (!_mode_auto) { _mode_auto = true; @@ -546,12 +546,12 @@ MulticopterPositionControl::control_auto(float dt) } } -void MulticopterPositionControl::handle_parameter_update(const px4_parameter_update &msg) +void MulticopterPositionControlMultiplatform::handle_parameter_update(const px4_parameter_update &msg) { parameters_update(); } -void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg) +void MulticopterPositionControlMultiplatform::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg) { /* Make sure that the position setpoint is valid */ if (!PX4_ISFINITE(_pos_sp_triplet->data().current.lat) || @@ -561,7 +561,7 @@ void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_posi } } -void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) +void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4_vehicle_attitude &msg) { static bool reset_int_z = true; static bool reset_int_z_manual = false; @@ -647,6 +647,21 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + /* make sure velocity setpoint is saturated in xy*/ + float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + + _vel_sp(1)*_vel_sp(1)); + if (vel_norm_xy > _params.vel_max(0)) { + /* note assumes vel_max(0) == vel_max(1) */ + _vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy; + _vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy; + } + + /* make sure velocity setpoint is saturated in z*/ + float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2)); + if (vel_norm_z > _params.vel_max(2)) { + _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; + } + if (!_control_mode->data().flag_control_altitude_enabled) { _reset_alt_sp = true; _vel_sp(2) = 0.0f; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 4f786c429801..d14d8b23e9fb 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -59,18 +59,18 @@ using namespace px4; -class MulticopterPositionControl +class MulticopterPositionControlMultiplatform { public: /** * Constructor */ - MulticopterPositionControl(); + MulticopterPositionControlMultiplatform(); /** * Destructor, also kills task. */ - ~MulticopterPositionControl(); + ~MulticopterPositionControlMultiplatform(); /* Callbacks for topics */ void handle_vehicle_attitude(const px4_vehicle_attitude &msg); diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp index 0b5775736e94..212f3a7acfae 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp @@ -46,18 +46,23 @@ #include "mc_pos_control.h" -bool thread_running = false; /**< Deamon status flag */ +bool mc_pos_control_thread_running = false; /**< Deamon status flag */ +#if defined(__PX4_ROS) int main(int argc, char **argv) +#else +int mc_pos_control_start_main(int argc, char **argv); // Prototype for missing declearation error with nuttx +int mc_pos_control_start_main(int argc, char **argv) +#endif { px4::init(argc, argv, "mc_pos_control_m"); PX4_INFO("starting"); - MulticopterPositionControl posctl; - thread_running = true; + MulticopterPositionControlMultiplatform posctl; + mc_pos_control_thread_running = true; posctl.spin(); PX4_INFO("exiting."); - thread_running = false; + mc_pos_control_thread_running = false; return 0; } diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp index 92416249d8f8..85d0dad3ff08 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -41,57 +41,57 @@ #include #include -extern bool thread_running; -int daemon_task; /**< Handle of deamon task / thread */ +extern bool mc_pos_control_thread_running; +int mc_pos_control_daemon_task; /**< Handle of deamon task / thread */ namespace px4 { -bool task_should_exit = false; +bool mc_pos_control_task_should_exit = false; } using namespace px4; -extern int main(int argc, char **argv); +extern int mc_pos_control_start_main(int argc, char **argv); extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]); int mc_pos_control_m_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: mc_pos_control_m {start|stop|status}"); + PX4_WARN("usage: mc_pos_control_m {start|stop|status}"); } if (!strcmp(argv[1], "start")) { - if (thread_running) { + if (mc_pos_control_thread_running) { warnx("already running"); /* this is not an error */ - exit(0); + return 0; } - task_should_exit = false; + mc_pos_control_task_should_exit = false; - daemon_task = px4_task_spawn_cmd("mc_pos_control_m", + mc_pos_control_daemon_task = px4_task_spawn_cmd("mc_pos_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2500, - main, + mc_pos_control_start_main, (argv) ? (char* const*)&argv[2] : (char* const*)NULL); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { - task_should_exit = true; - exit(0); + mc_pos_control_task_should_exit = true; + return 0; } if (!strcmp(argv[1], "status")) { - if (thread_running) { + if (mc_pos_control_thread_running) { warnx("is running"); } else { warnx("not started"); } - exit(0); + return 0; } warnx("unrecognized command"); diff --git a/src/modules/muorb/adsp/CMakeLists.txt b/src/modules/muorb/adsp/CMakeLists.txt new file mode 100644 index 000000000000..185264a85f2c --- /dev/null +++ b/src/modules/muorb/adsp/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +include_directories("../../uORB") + +px4_add_module( + MODULE modules__muorb__adsp + COMPILE_FLAGS + -Os + SRCS + px4muorb.cpp + uORBFastRpcChannel.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/muorb/adsp/module.mk b/src/modules/muorb/adsp/module.mk new file mode 100644 index 000000000000..d31ca360dbf7 --- /dev/null +++ b/src/modules/muorb/adsp/module.mk @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# 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 muorb +# + + +ifeq ($(PX4_TARGET_OS),qurt) + +SRCS = \ + px4muorb.cpp \ + uORBFastRpcChannel.cpp + +INCLUDE_DIRS += \ + ${PX4_BASE}/src/modules/uORB + +endif + +MAXOPTIMIZATION = -Os diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp new file mode 100644 index 000000000000..df951eb967d1 --- /dev/null +++ b/src/modules/muorb/adsp/px4muorb.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#include "px4muorb.hpp" +#include "uORBFastRpcChannel.hpp" +#include "uORBManager.hpp" + +#include +#include +#include +#include +#include "px4_log.h" +#include "uORB/topics/sensor_combined.h" +#include "uORB.h" + +__BEGIN_DECLS +extern int dspal_main(int argc, char *argv[]); +__END_DECLS + +int px4muorb_orb_initialize() +{ + HAP_power_request(100, 100, 1000); + // register the fastrpc muorb with uORBManager. + uORB::Manager::get_instance()->set_uorb_communicator(uORB::FastRpcChannel::GetInstance()); + const char *argv[] = { "dspal", "start" }; + int argc = 2; + int rc; + rc = dspal_main(argc, (char **)argv); + return rc; +} + +int px4muorb_add_subscriber(const char *name) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + channel->AddRemoteSubscriber(name); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_add_subscription(name, 0); + + if (rc != OK) { + channel->RemoveRemoteSubscriber(name); + } + + } else { + rc = -1; + } + + return rc; +} + +int px4muorb_remove_subscriber(const char *name) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + channel->RemoveRemoteSubscriber(name); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_remove_subscription(name); + + } else { + rc = -1; + } + + return rc; +} + +int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_received_message(name, data_len_in_bytes, (uint8_t *)data); + + } else { + rc = -1; + } + + return rc; +} + +int px4muorb_is_subscriber_present(const char *topic_name, int *status) +{ + int rc = 0; + int32_t local_status = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->is_subscriber_present(topic_name, &local_status); + + if (rc == 0) { + *status = (int)local_status; + } + + return rc; +} + +int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, + int *bytes_returned) +{ + int rc = 0; + int32_t local_msg_type = 0; + int32_t local_bytes_returned = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data ); + rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data, data_len_in_bytes, &local_bytes_returned); + *msg_type = (int)local_msg_type; + *bytes_returned = (int)local_bytes_returned; + return rc; +} + +int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer, int max_size_in_bytes, + int *returned_length_in_bytes, int *topic_count) +{ + int rc = 0; + int32_t local_bytes_returned = 0; + int32_t local_topic_count = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data ); + rc = channel->get_bulk_data(bulk_transfer_buffer, max_size_in_bytes, &local_bytes_returned, &local_topic_count); + *returned_length_in_bytes = (int)local_bytes_returned; + *topic_count = (int)local_topic_count; + return rc; +} + +int px4muorb_unblock_recieve_msg(void) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->unblock_get_data_method(); + return rc; +} diff --git a/src/modules/muorb/adsp/px4muorb.hpp b/src/modules/muorb/adsp/px4muorb.hpp new file mode 100644 index 000000000000..1ec6b45566f0 --- /dev/null +++ b/src/modules/muorb/adsp/px4muorb.hpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 + +#include +#include + +extern "C" { + + int px4muorb_orb_initialize() __EXPORT; + + int px4muorb_add_subscriber(const char *name) __EXPORT; + + int px4muorb_remove_subscriber(const char *name) __EXPORT; + + int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; + + int px4muorb_is_subscriber_present(const char *topic_name, int *status) __EXPORT; + + int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, + int *bytes_returned) __EXPORT; + + int px4muorb_receive_bulk_data(uint8_t *_BulkTransferBuffer, int max_size_in_bytes, + int *length_in_bytes, int *topic_count) __EXPORT; + + int px4muorb_unblock_recieve_msg(void) __EXPORT; + +} diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp new file mode 100644 index 000000000000..82a330132298 --- /dev/null +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp @@ -0,0 +1,553 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#include "uORBFastRpcChannel.hpp" +#include "px4_log.h" +#include +#include + +// static intialization. +uORB::FastRpcChannel uORB::FastRpcChannel::_Instance; + +static hrt_abstime _check_time; +static unsigned long _dropped_pkts; +static unsigned long _get_min = 0xFFFFFF; +static unsigned long _get_max = 0; +static unsigned long _min_q = 200; +static unsigned long _max_q = 0; +static unsigned long _avg_q = 0; +static unsigned long _count = 0; +static unsigned long _get_bulk_min = 0xFFFFFF; +static unsigned long _get_bulk_max = 0; +static unsigned long _bulk_topic_count_min = 0xFFFFFF; +static unsigned long _bulk_topic_count_max = 0; + +//============================================================================== +//============================================================================== +uORB::FastRpcChannel::FastRpcChannel() + : _RxHandler(0) + , _DataQInIndex(0) + , _DataQOutIndex(0) + , _ControlQInIndex(0) + , _ControlQOutIndex(0) +{ + for (int32_t i = 0; i < _MAX_MSG_QUEUE_SIZE; ++ i) { + _DataMsgQueue[i]._MaxBufferSize = 0; + _DataMsgQueue[i]._Length = 0; + _DataMsgQueue[i]._Buffer = 0; + } + + _RemoteSubscribers.clear(); +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) +{ + int16_t rc = 0; + _Subscribers.push_back(messageName); + PX4_DEBUG("Adding message[%s] to subscriber queue...", messageName); + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::remove_subscription(const char *messageName) +{ + int16_t rc = 0; + _Subscribers.remove(messageName); + + return rc; +} + +int16_t uORB::FastRpcChannel::is_subscriber_present(const char *messageName, int32_t *status) +{ + int16_t rc = 0; + + if (std::find(_Subscribers.begin(), _Subscribers.end(), messageName) != _Subscribers.end()) { + *status = 1; + //PX4_DEBUG("******* Found subscriber for message[%s]....", messageName); + + } else { + *status = 0; + //PX4_WARN("@@@@@ Subscriber not found for[%s]...numSubscribers[%d]", messageName, _Subscribers.size()); + int i = 0; + + for (std::list::iterator it = _Subscribers.begin(); it != _Subscribers.end(); ++it) { + if (*it == messageName) { + PX4_DEBUG("##### Found the message[%s] in the subscriber list-index[%d]", messageName, i); + } + + ++i; + } + } + + return rc; +} + +int16_t uORB::FastRpcChannel::unblock_get_data_method() +{ + PX4_DEBUG("[unblock_get_data_method] calling post method for _DataAvailableSemaphore()"); + _DataAvailableSemaphore.post(); + return 0; +} +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) +{ + _RxHandler = handler; + return 0; +} + + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) +{ + int16_t rc = 0; + hrt_abstime t1, t2; + static hrt_abstime check_time = 0; + int32_t initial_queue_size = 0; + + if (_RemoteSubscribers.find(messageName) == _RemoteSubscribers.end()) { + //there is no-remote subscriber. So do not queue the message. + return rc; + } + + t1 = hrt_absolute_time(); + _QueueMutex.lock(); + bool overwriteData = false; + + if (IsDataQFull()) { + // queue is full. Overwrite the oldest data. + //PX4_WARN("[send_message] Queue Full Overwrite the oldest data. in[%ld] out[%ld] max[%ld]", + // _DataQInIndex, _DataQOutIndex, _MAX_MSG_QUEUE_SIZE); + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + + overwriteData = true; + _dropped_pkts++; + } + + // now check to see if the data queue's buffer size if large enough to memcpy the data. + // if not, delete the old buffer and re-create a new buffer of larger size. + check_and_expand_data_buffer(_DataQInIndex, length); + + // now memcpy the data to the buffer. + memcpy(_DataMsgQueue[ _DataQInIndex ]._Buffer, data, length); + _DataMsgQueue[ _DataQInIndex ]._Length = length; + _DataMsgQueue[ _DataQInIndex ]._MsgName = messageName; + + _DataQInIndex++; + + if (_DataQInIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQInIndex = 0; + } + + // the assumption here is that each caller reads only one data from either control or data queue. + //if (!overwriteData) { + if (DataQSize() == 1) { // post it only of the queue moves from empty to available. + _DataAvailableSemaphore.post(); + } + + if ((unsigned long)DataQSize() < _min_q) { _min_q = (unsigned long)DataQSize(); } + + if ((unsigned long)DataQSize() > _max_q) { _max_q = (unsigned long)DataQSize(); } + + _count++; + _avg_q = ((double)((_avg_q * (_count - 1)) + (unsigned long)(DataQSize()))) / (double)(_count); + + _QueueMutex.unlock(); + t2 = hrt_absolute_time(); + + if ((unsigned long)(t2 - check_time) > 10000000) { + //PX4_DEBUG("MsgName: %20s, t1: %lu, t2: %lu, dt: %lu",messageName, (unsigned long) t1, (unsigned long) t2, (unsigned long) (t2-t1)); + //PX4_DEBUG("Q. Stats: min: %lu, max : %lu, avg: %lu count: %lu ", _min_q, _max_q, (unsigned long)(_avg_q * 1000.0), _count); + _min_q = _MAX_MSG_QUEUE_SIZE * 2; + _max_q = 0; + _avg_q = 0; + _count = 0; + check_time = t2; + } + + return rc; +} + +//============================================================================== +//============================================================================== +void uORB::FastRpcChannel::check_and_expand_data_buffer(int32_t index, int32_t length) +{ + if (_DataMsgQueue[ index ]._MaxBufferSize < length) { + // create a new buffer of size length and delete old buffer. + if (_DataMsgQueue[ index ]._Buffer != 0) { + delete _DataMsgQueue[ index ]._Buffer; + } + + _DataMsgQueue[ index ]._Buffer = new uint8_t[ length ]; + + if (_DataMsgQueue[ index ]._Buffer == 0) { + PX4_ERR("Error[check_and_expand_data_buffer] Failed to allocate data queue buffer of size[%ld]", length); + _DataMsgQueue[ index ]._MaxBufferSize = 0; + return; + } + + _DataMsgQueue[ index ]._MaxBufferSize = length; + } +} + +int32_t uORB::FastRpcChannel::DataQSize() +{ + int32_t rc; + rc = (_DataQInIndex - _DataQOutIndex) + _MAX_MSG_QUEUE_SIZE; + rc %= _MAX_MSG_QUEUE_SIZE; + return rc; +} + +int32_t uORB::FastRpcChannel::ControlQSize() +{ + int32_t rc; + rc = (_ControlQInIndex - _ControlQOutIndex) + _MAX_MSG_QUEUE_SIZE; + rc %= _MAX_MSG_QUEUE_SIZE; + return rc; +} + +bool uORB::FastRpcChannel::IsControlQFull() +{ + return (ControlQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); +} + +bool uORB::FastRpcChannel::IsControlQEmpty() +{ + return (ControlQSize() == 0); +} + +bool uORB::FastRpcChannel::IsDataQFull() +{ + return (DataQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); +} + +bool uORB::FastRpcChannel::IsDataQEmpty() +{ + return (DataQSize() == 0); +} + +int16_t uORB::FastRpcChannel::get_data +( + int32_t *msg_type, + char *topic_name, + int32_t topic_name_len, + uint8_t *data, + int32_t data_len_in_bytes, + int32_t *bytes_returned +) +{ + int16_t rc = 0; + PX4_DEBUG("Get data should not be called..."); + return -1; + // wait for data availability + static hrt_abstime check_time = 0; + hrt_abstime t1 = hrt_absolute_time(); + _DataAvailableSemaphore.wait(); + hrt_abstime t2 = hrt_absolute_time(); + _QueueMutex.lock(); + + if (DataQSize() != 0 || ControlQSize() != 0) { + if (ControlQSize() > 0) { + // read the first element of the Control Queue. + *msg_type = _ControlMsgQueue[ _ControlQOutIndex ]._Type; + + if ((int)_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() < (int)topic_name_len) { + memcpy + ( + topic_name, + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.c_str(), + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() + ); + + topic_name[_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()] = 0; + + *bytes_returned = 0; + + _ControlQOutIndex++; + + if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _ControlQOutIndex = 0; + } + + } else { + PX4_ERR("Error[get_data-CONTROL]: max topic_name_len[%ld] < controlMsgLen[%d]", + topic_name_len, + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() + ); + rc = -1; + } + + } else { + // read the first element of the Control Queue. + *msg_type = _DATA_MSG_TYPE; + + if (((int)_DataMsgQueue[ _DataQOutIndex ]._MsgName.size() < topic_name_len) || + (_DataMsgQueue[ _DataQOutIndex ]._Length < data_len_in_bytes)) { + memcpy + ( + topic_name, + _DataMsgQueue[ _DataQOutIndex ]._MsgName.c_str(), + _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() + ); + + topic_name[_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()] = 0; + + *bytes_returned = _DataMsgQueue[ _DataQOutIndex ]._Length; + memcpy(data, _DataMsgQueue[ _DataQOutIndex ]._Buffer, _DataMsgQueue[ _DataQOutIndex ]._Length); + + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + + } else { + PX4_ERR("Error:[get_data-DATA] type msg max topic_name_len[%ld] > dataMsgLen[%d] ", + topic_name_len, + _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() + ); + PX4_ERR("Error:[get_data-DATA] Or data_buffer_len[%ld] > message_size[%ld] ", + data_len_in_bytes, + _DataMsgQueue[ _DataQOutIndex ]._Length + ); + + rc = -1; + } + } + + } else { + PX4_ERR("[get_data] Error: Semaphore is up when there is no data on the control/data queues"); + rc = -1; + } + + _QueueMutex.unlock(); + hrt_abstime t3 = hrt_absolute_time(); + + if ((unsigned long)(t3 - t1) > _get_max) { _get_max = (unsigned long)(t3 - t1); } + + if ((unsigned long)(t3 - t1) < _get_min) { _get_min = (unsigned long)(t3 - t1); } + + if ((unsigned long)(t3 - check_time) > 1000000) { + if (rc != 0) { + topic_name[0] = '\0'; + } + + /* + PX4_DEBUG("GetData: %30s: t1: %lu t2: %lu t3: %lu", topic_name, (unsigned long)t1, (unsigned long)t2, + (unsigned long)t3); + PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize()); + PX4_DEBUG("ADSP RPC Stats: _get_min: %lu _get_max: %lu _dropped_pkts: %lu", _get_min, _get_max, _dropped_pkts); + */ + check_time = t3; + } + + return rc; +} + + +int16_t uORB::FastRpcChannel::get_bulk_data +( + uint8_t *buffer, + int32_t max_buffer_in_bytes, + int32_t *returned_bytes, + int32_t *topic_count +) +{ + int16_t rc = 0; + // wait for data availability + static hrt_abstime check_time = 0; + hrt_abstime t1 = hrt_absolute_time(); + _DataAvailableSemaphore.wait(); + hrt_abstime t2 = hrt_absolute_time(); + + _QueueMutex.lock(); + + int32_t bytes_copied = 0; + int32_t copy_result = 0; + *returned_bytes = 0; + *topic_count = 0; + int32_t topic_count_to_return = 0; + + if (DataQSize() != 0) { + //PX4_DEBUG( "get_bulk_data: QSize: %d", DataQSize() ); + topic_count_to_return = DataQSize(); + + while (DataQSize() != 0) { + // this is a hack as we are using a counting semaphore. Should be re-implemented with cond_variable and wait. + //_DataAvailableSemaphore.wait(); + if (get_data_msg_size_at(_DataQOutIndex) < (max_buffer_in_bytes - bytes_copied)) { + // there is enough space in the buffer, copy the data. + //PX4_DEBUG( "Coping Data to buffer..." ); + copy_result = copy_data_to_buffer(_DataQOutIndex, buffer, bytes_copied, max_buffer_in_bytes); + + if (copy_result == -1) { + if (bytes_copied == 0) { + rc = -1; + } + + break; + + } else { + //PX4_DEBUG( "[%d] %02x %02x %02x %02x", *topic_count,\ + // buffer[bytes_copied], \ + // buffer[bytes_copied+1], \ + // buffer[bytes_copied+2], \ + // buffer[bytes_copied+3] ); + bytes_copied += copy_result; + (*topic_count)++; + *returned_bytes = bytes_copied; + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + } + + } else { + if (bytes_copied == 0) { + rc = -1; + PX4_WARN("ERROR: Insufficent space in data buffer, no topics returned"); + + } else { + PX4_DEBUG("Exiting out of the while loop..."); + } + + break; + } + } + + } else { + PX4_ERR("[get_data_bulk] Error: Semaphore is up when there is no data on the control/data queues"); + rc = -1; + } + + if (topic_count_to_return != *topic_count) { + PX4_WARN("Not sending all topics: topics_to_return:[%ld] topics_returning:[%ld]", topic_count_to_return, *topic_count); + } + + _QueueMutex.unlock(); + hrt_abstime t3 = hrt_absolute_time(); + + if ((unsigned long)(t3 - t1) > _get_bulk_max) { _get_bulk_max = (unsigned long)(t3 - t1); } + + if ((unsigned long)(t3 - t1) < _get_bulk_min) { _get_bulk_min = (unsigned long)(t3 - t1); } + + if ((unsigned long)(*topic_count) > _bulk_topic_count_max) { _bulk_topic_count_max = (unsigned long)(*topic_count); } + + if ((unsigned long)(*topic_count) < _bulk_topic_count_min) { _bulk_topic_count_min = (unsigned long)(*topic_count); } + + if ((unsigned long)(t3 - check_time) > 10000000) { + //PX4_DEBUG("GetData: t1: %lu t2: %lu t3: %lu", (unsigned long)t1, (unsigned long)t2, (unsigned long)t3); + //PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize()); + //PX4_DEBUG("ADSP RPC Stats: _get_bulk_min: %lu _get_bulk_max: %lu _dropped_pkts: %lu", _get_bulk_min, _get_bulk_max, + // _dropped_pkts); + //PX4_DEBUG(" .... topic_count_min: %lu topic_count_max: %lu", _bulk_topic_count_min, _bulk_topic_count_max); + _get_bulk_max = 0; + _get_bulk_min = 0xFFFFFF; + _bulk_topic_count_min = 0xFFFFFF; + _bulk_topic_count_max = 0; + check_time = t3; + } + + //PX4_DEBUG( "Returning topics: %d bytes_returned: %d", *topic_count, *returned_bytes ); + return rc; +} + + +int32_t uORB::FastRpcChannel::get_data_msg_size_at(int32_t index) +{ + // the assumption here is that this is called within the context of semaphore, + // hence lock/unlock is not needed. + int32_t rc = 0; + rc += _DataMsgQueue[ index ]._Length; + rc += _DataMsgQueue[ index ]._MsgName.size() + 1; + rc += _PACKET_HEADER_SIZE; + return rc; +} + + +int32_t uORB::FastRpcChannel::copy_data_to_buffer(int32_t src_index, uint8_t *dst_buffer, int32_t offset, + int32_t dst_buffer_len) +{ + int32_t rc = -1; + + // before calling this method the following are assumed: + // * sem_lock is acquired for data protection + // * the dst_buffer is validated to + + // compute the different offsets to pack the packets. + int32_t field_header_offset = offset; + int32_t field_topic_name_offset = field_header_offset + sizeof(struct BulkTransferHeader); + int32_t field_data_offset = field_topic_name_offset + _DataMsgQueue[ src_index ]._MsgName.size() + 1; + + struct BulkTransferHeader header = { (uint16_t)(_DataMsgQueue[ src_index ]._MsgName.size() + 1), (uint16_t)(_DataMsgQueue[ src_index ]._Length) }; + + + //PX4_DEBUG( "Offsets: header[%d] name[%d] data[%d]", + // field_header_offset, + // field_topic_name_offset, + // field_data_offset ); + + if ((field_data_offset + _DataMsgQueue[ src_index ]._Length) < dst_buffer_len) { + memmove(&(dst_buffer[field_header_offset]), (char *)(&header), sizeof(header)); + // pack the data here. + memmove + ( + &(dst_buffer[field_topic_name_offset]), + _DataMsgQueue[ src_index ]._MsgName.c_str(), + _DataMsgQueue[ src_index ]._MsgName.size() + ); + + if (_DataMsgQueue[ src_index ]._MsgName.size() == 0) { + PX4_WARN("########## Error MsgName cannot be zero: "); + } + + dst_buffer[ field_topic_name_offset + _DataMsgQueue[ src_index ]._MsgName.size()] = '\0'; + memmove(&(dst_buffer[field_data_offset]), _DataMsgQueue[ src_index ]._Buffer, _DataMsgQueue[ src_index ]._Length); + rc = field_data_offset + _DataMsgQueue[ src_index ]._Length - offset; + + } else { + PX4_WARN("Error coping the DataMsg to dst buffer, insuffienct space. "); + PX4_WARN("... offset[%ld] len[%ld] data_msg_len[%ld]", + offset, dst_buffer_len, (field_data_offset - offset) + _DataMsgQueue[ src_index ]._Length); + } + + return rc; +} diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.hpp b/src/modules/muorb/adsp/uORBFastRpcChannel.hpp new file mode 100644 index 000000000000..37955603fde5 --- /dev/null +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.hpp @@ -0,0 +1,267 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#ifndef _uORBFastRpcChannel_hpp_ +#define _uORBFastRpcChannel_hpp_ + +#include +#include +#include +#include "uORB/uORBCommunicator.hpp" +#include +#include + +namespace uORB +{ +class FastRpcChannel; +} + +class uORB::FastRpcChannel : public uORBCommunicator::IChannel +{ +public: + /** + * static method to get the IChannel Implementor. + */ + static uORB::FastRpcChannel *GetInstance() + { + return &(_Instance); + } + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription(const char *messageName); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); + + //Function to return the data to krait. + int16_t get_data + ( + int32_t *msg_type, + char *topic_name, + int32_t topic_name_len, + uint8_t *data, + int32_t data_len_in_bytes, + int32_t *bytes_returned + ); + + int16_t get_bulk_data(uint8_t *buffer, int32_t max_size_in_bytes, int32_t *returned_bytes, int32_t *topic_count); + + // function to check if there are subscribers for a topic on adsp. + int16_t is_subscriber_present(const char *messageName, int32_t *status); + + // function to release the blocking semaphore for get_data method. + int16_t unblock_get_data_method(); + + uORBCommunicator::IChannelRxHandler *GetRxHandler() + { + return _RxHandler; + } + + void AddRemoteSubscriber(const std::string &messageName) + { + _RemoteSubscribers.insert(messageName); + } + void RemoveRemoteSubscriber(const std::string &messageName) + { + _RemoteSubscribers.erase(messageName); + } + +private: // data members + static uORB::FastRpcChannel _Instance; + uORBCommunicator::IChannelRxHandler *_RxHandler; + + /// data structure to store the messages to be retrived by Krait. + static const int32_t _MAX_MSG_QUEUE_SIZE = 100; + static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; + static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; + static const int32_t _DATA_MSG_TYPE = 3; + + static const int32_t _PACKET_FIELD_TOPIC_NAME_LEN_SIZE_IN_BYTES = 2; + static const int32_t _PACKET_FIELD_DATA_LEN_IN_BYTES = 2; + static const int32_t _PACKET_HEADER_SIZE = + _PACKET_FIELD_TOPIC_NAME_LEN_SIZE_IN_BYTES + _PACKET_FIELD_DATA_LEN_IN_BYTES; + + struct FastRpcDataMsg { + int32_t _MaxBufferSize; + int32_t _Length; + uint8_t *_Buffer; + std::string _MsgName; + }; + + struct FastRpcControlMsg { + int32_t _Type; + std::string _MsgName; + }; + + struct BulkTransferHeader { + uint16_t _MsgNameLen; + uint16_t _DataLen; + }; + + + struct FastRpcDataMsg _DataMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; + int32_t _DataQInIndex; + int32_t _DataQOutIndex; + + struct FastRpcControlMsg _ControlMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; + int32_t _ControlQInIndex; + int32_t _ControlQOutIndex; + + std::list _Subscribers; + + //utility classes + class Mutex + { + public: + Mutex() + { + sem_init(&_Sem, 0, 1); + } + ~Mutex() + { + sem_destroy(&_Sem); + } + void lock() + { + sem_wait(&_Sem); + } + void unlock() + { + sem_post(&_Sem); + } + private: + sem_t _Sem; + + Mutex(const Mutex &); + + Mutex &operator=(const Mutex &); + }; + + class Semaphore + { + public: + Semaphore() + { + sem_init(&_Sem, 0, 0); + } + ~Semaphore() + { + sem_destroy(&_Sem); + } + void post() + { + sem_post(&_Sem); + } + void wait() + { + sem_wait(&_Sem); + } + private: + sem_t _Sem; + Semaphore(const Semaphore &); + Semaphore &operator=(const Semaphore &); + + }; + + Mutex _QueueMutex; + Semaphore _DataAvailableSemaphore; + +private://class members. + /// constructor. + FastRpcChannel(); + + void check_and_expand_data_buffer(int32_t index, int32_t length); + + bool IsControlQFull(); + bool IsControlQEmpty(); + bool IsDataQFull(); + bool IsDataQEmpty(); + int32_t DataQSize(); + int32_t ControlQSize(); + + int32_t get_data_msg_size_at(int32_t index); + int32_t copy_data_to_buffer(int32_t src_index, uint8_t *dst_buffer, int32_t offset, int32_t dst_buffer_len); + + std::set _RemoteSubscribers; +}; + +#endif /* _uORBFastRpcChannel_hpp_ */ diff --git a/src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp b/src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp new file mode 100644 index 000000000000..f6b64b8e5454 --- /dev/null +++ b/src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#ifndef _px4muorb_KraitRpcWrapper_hpp_ +#define _px4muorb_KraitRpcWrapper_hpp_ +#include + +namespace px4muorb +{ +class KraitRpcWrapper; +} + +class px4muorb::KraitRpcWrapper +{ +public: + /** + * Constructor + */ + KraitRpcWrapper() {} + + /** + * destructor + */ + ~KraitRpcWrapper() {} + + /** + * Initiatizes the rpc channel px4 muorb + */ + bool Initialize() { return true; } + + /** + * Terminate to clean up the resources. This should be called at program exit + */ + bool Terminate() { return true; } + + /** + * Muorb related functions to pub/sub of orb topic from krait to adsp + */ + int32_t AddSubscriber(const char *topic) { return 1; } + int32_t RemoveSubscriber(const char *topic) { return 1; } + int32_t SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data) { return 1; } + int32_t ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, uint8_t **data) { return 1; } + int32_t IsSubscriberPresent(const char *topic, int32_t *status) { return 1; } + int32_t ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count) { return 1; } + int32_t UnblockReceiveData() { return 1; } +}; +#endif // _px4muorb_KraitWrapper_hpp_ diff --git a/src/modules/muorb/krait/CMakeLists.txt b/src/modules/muorb/krait/CMakeLists.txt new file mode 100644 index 000000000000..1c1d6605ac2a --- /dev/null +++ b/src/modules/muorb/krait/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if("${DSPAL_STUBS_ENABLE}" STREQUAL "1") + include_directories(../krait-stubs) +endif() + +px4_add_module( + MODULE modules__muorb__krait + MAIN muorb + SRCS + uORBKraitFastRpcChannel.cpp + muorb_main.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/muorb/krait/module.mk b/src/modules/muorb/krait/module.mk new file mode 100644 index 000000000000..4f30ee46e1b2 --- /dev/null +++ b/src/modules/muorb/krait/module.mk @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# 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 uORB +# + +MODULE_COMMAND = muorb + +ifeq ($(PX4_TARGET_OS),posix-arm) +SRCS = uORBKraitFastRpcChannel.cpp \ + muorb_main.cpp +INCLUDE_DIRS += $(EXT_MUORB_LIB_ROOT)/krait/include \ + $(PX4_BASE)/src/modules/uORB \ + $(PX4_BASE)/src/modules + +EXTRA_LIBS += $(EXT_MUORB_LIB_ROOT)/krait/libs/libpx4muorb.so +endif diff --git a/src/modules/muorb/krait/muorb_main.cpp b/src/modules/muorb/krait/muorb_main.cpp new file mode 100644 index 000000000000..006870780c0f --- /dev/null +++ b/src/modules/muorb/krait/muorb_main.cpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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. + * + ****************************************************************************/ + +#include +#include "modules/uORB/uORBManager.hpp" +#include "uORBKraitFastRpcChannel.hpp" + +extern "C" { __EXPORT int muorb_main(int argc, char *argv[]); } + +static void usage() +{ + warnx("Usage: muorb 'start', 'stop', 'status'"); +} + + +int +muorb_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return -EINVAL; + } + + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + // register the fast rpc channel with UORB. + uORB::Manager::get_instance()->set_uorb_communicator(uORB::KraitFastRpcChannel::GetInstance()); + + // start the KaitFastRPC channel thread. + uORB::KraitFastRpcChannel::GetInstance()->Start(); + return OK; + + } + + if (!strcmp(argv[1], "stop")) { + + uORB::KraitFastRpcChannel::GetInstance()->Stop(); + return OK; + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "status")) { + return OK; + } + + usage(); + return -EINVAL; +} diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp new file mode 100644 index 000000000000..6e53f0bbfd7a --- /dev/null +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -0,0 +1,345 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ + +#include "uORBKraitFastRpcChannel.hpp" +#include "px4_log.h" +#include "px4_tasks.h" +#include +#include + +#define LOG_TAG "uORBKraitFastRpcChannel.cpp" + +uORB::KraitFastRpcChannel uORB::KraitFastRpcChannel::_Instance; + +static void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics); + +// static intialization. +static std::string _log_file_name = "./hex_dump.txt"; + +static unsigned long _snd_msg_min = 0xFFFFFF; +static unsigned long _snd_msg_max = 0; +static double _snd_msg_avg = 0.0; +static unsigned long _snd_msg_count = 0; +static unsigned long _overall_snd_min = 0xFFFFFF; +static unsigned long _overall_snd_max = 0; +static double _overall_snd_avg = 0.0; +static unsigned long _overall_snd_count = 0; +static hrt_abstime _log_check_time = 0; +static hrt_abstime _log_check_interval = 10000000; + + +uORB::KraitFastRpcChannel::KraitFastRpcChannel() : + _RxHandler(nullptr), + _ThreadStarted(false), + _ThreadShouldExit(false) +{ + _KraitWrapper.Initialize(); +} + +int16_t uORB::KraitFastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) +{ + int16_t rc = 0; + //PX4_DEBUG("Before calling AddSubscriber for [%s]\n", messageName); + rc = _KraitWrapper.AddSubscriber(messageName); + //PX4_DEBUG("Response for AddSubscriber for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +int16_t uORB::KraitFastRpcChannel::remove_subscription(const char *messageName) +{ + int16_t rc = 0; + //PX4_DEBUG("Before calling RemoveSubscriber for [%s]\n", messageName); + rc = _KraitWrapper.RemoveSubscriber(messageName); + //PX4_DEBUG("Response for RemoveSubscriber for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +int16_t uORB::KraitFastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) +{ + _RxHandler = handler; + return 0; +} + +int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) +{ + int16_t rc = 0; + int32_t status = 0; + hrt_abstime t1, t4; + hrt_abstime t2 = 0; + hrt_abstime t3 = 0; + t1 = hrt_absolute_time(); + + if (_AdspSubscriberCache.find(std::string(messageName)) == _AdspSubscriberCache.end()) { + // check the status from adsp. as it is not cached. + if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) { + _AdspSubscriberCache[messageName] = status; + _AdspSubscriberSampleTimestamp[messageName] = hrt_absolute_time(); + } + + } else { + if ((hrt_absolute_time() - _AdspSubscriberSampleTimestamp[messageName]) > _SubCacheRefreshRate) { + if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) { + _AdspSubscriberCache[messageName] = status; + _AdspSubscriberSampleTimestamp[messageName] = hrt_absolute_time(); + } + } + } + + if (_AdspSubscriberCache[messageName] > 0) {// there are remote subscribers + t2 = hrt_absolute_time(); + rc = _KraitWrapper.SendData(messageName, length, data); + t3 = hrt_absolute_time(); + _snd_msg_count++; + //PX4_DEBUG( "***** SENDING[%s] topic to remote....\n", messageName.c_str() ); + + } else { + //PX4_DEBUG( "******* NO SUBSCRIBER PRESENT ON THE REMOTE FOR topic[%s] \n", messageName.c_str() ); + } + + t4 = hrt_absolute_time(); + _overall_snd_count++; + + if ((t4 - t1) < _overall_snd_min) { _overall_snd_min = (t4 - t1); } + + if ((t4 - t1) > _overall_snd_max) { _overall_snd_max = (t4 - t1); } + + if (_AdspSubscriberCache[messageName] > 0) { + if ((t3 - t2) < _snd_msg_min) { _snd_msg_min = (t3 - t2); } + + if ((t3 - t2) > _snd_msg_max) { _snd_msg_max = (t3 - t2); } + + _snd_msg_avg = ((double)((_snd_msg_avg * (_snd_msg_count - 1)) + + (unsigned long)(t3 - t2))) / (double)(_snd_msg_count); + } + + _overall_snd_avg = ((double)((_overall_snd_avg * (_overall_snd_count - 1)) + + (unsigned long)(t4 - t1))) / (double)(_overall_snd_count); + + if ((t4 - _log_check_time) > _log_check_interval) { + /* + PX4_DEBUG("SndMsgStats: overall_min: %lu overall_max: %lu snd_msg_min: %lu snd_msg_max: %lu", + _overall_snd_min, _overall_snd_max, + _snd_msg_min, _snd_msg_max); + PX4_DEBUG(".... overall_avg: %f (%lu) snd_msg_avg: %f (%lu)", + _overall_snd_avg, _overall_snd_count, _snd_msg_avg, _snd_msg_count); + */ + _log_check_time = t4; + _overall_snd_min = _snd_msg_min = 0xFFFFFFF; + _overall_snd_max = _snd_msg_max = 0; + _overall_snd_count = _snd_msg_count = 0; + _overall_snd_avg = _snd_msg_avg = 0.0; + } + + //PX4_DEBUG( "Response for SendMessage for [%s],len[%d] rc[%d]\n", messageName.c_str(), length, rc ); + return rc; +} + +void uORB::KraitFastRpcChannel::Start() +{ + _ThreadStarted = true; + _ThreadShouldExit = false; + pthread_attr_t recv_thread_attr; + pthread_attr_init(&recv_thread_attr); + + struct sched_param param; + (void)pthread_attr_getschedparam(&recv_thread_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_MAX - 80; + (void)pthread_attr_setschedparam(&recv_thread_attr, ¶m); + + pthread_attr_setstacksize(&recv_thread_attr, 4096); + + if (pthread_create(&_RecvThread, &recv_thread_attr, thread_start, (void *)this) != 0) { + PX4_ERR("Error creating the receive thread for muorb"); + + } else { + pthread_setname_np(_RecvThread, "muorb_krait_receiver"); + } + + pthread_attr_destroy(&recv_thread_attr); +} + +void uORB::KraitFastRpcChannel::Stop() +{ + _ThreadShouldExit = true; + _KraitWrapper.UnblockReceiveData(); + //PX4_DEBUG("After calling UnblockReceiveData()...\n"); + pthread_join(_RecvThread, NULL); + //PX4_DEBUG("*** After calling pthread_join...\n"); + _ThreadStarted = false; +} + +void *uORB::KraitFastRpcChannel::thread_start(void *handler) +{ + if (handler != nullptr) { + ((uORB::KraitFastRpcChannel *)handler)->fastrpc_recv_thread(); + } + + return 0; +} + +void uORB::KraitFastRpcChannel::fastrpc_recv_thread() +{ + // sit in while loop. + int32_t rc = 0; + int32_t data_length = 0; + uint8_t *data = nullptr; + unsigned long rpc_min, rpc_max; + unsigned long orb_min, orb_max; + double rpc_avg, orb_avg; + unsigned long count = 0; + rpc_max = orb_max = 0; + rpc_min = orb_min = 0xFFFFFFFF; + rpc_avg = orb_avg = 0.0; + + int32_t num_topics = 0; + + hrt_abstime check_time = 0; + + while (!_ThreadShouldExit) { + hrt_abstime t1, t2, t3; + t1 = hrt_absolute_time(); + rc = _KraitWrapper.ReceiveBulkData(&data, &data_length, &num_topics); + + t2 = hrt_absolute_time(); + + if (rc == 0) { + //PX4_DEBUG( "Num of topics Received: %d", num_topics ); + int32_t bytes_processed = 0; + + for (int i = 0; i < num_topics; ++i) { + uint8_t *new_pkt = &(data[bytes_processed]); + struct BulkTransferHeader *header = (struct BulkTransferHeader *)new_pkt; + char *messageName = (char *)(new_pkt + sizeof(struct BulkTransferHeader)); + uint16_t check_msg_len = strlen(messageName); + + if (header->_MsgNameLen != (check_msg_len + 1)) { + PX4_ERR("Error: Packing error. Sent Msg Len. of[%d] but strlen returned:[%d]", header->_MsgNameLen , check_msg_len); + PX4_ERR("Error: NumTopics: %d processing topic: %d msgLen[%d] dataLen[%d] data_len[%d] bytes processed: %d", + num_topics, i, header->_MsgNameLen, header->_DataLen , data_length, bytes_processed); + DumpData(data, data_length, num_topics); + break; + } + + uint8_t *topic_data = (uint8_t *)(messageName + strlen(messageName) + 1); + + if (_RxHandler != nullptr) { + _RxHandler->process_received_message(messageName, + header->_DataLen, topic_data); + //PX4_DEBUG( "Received topic data for control message for: [%s] len[%d]\n", name, data_length ); + } + + bytes_processed += header->_MsgNameLen + header->_DataLen + sizeof(struct BulkTransferHeader); + } + + } else { + PX4_DEBUG("Error: Getting data over fastRPC channel\n"); + break; + } + + t3 = hrt_absolute_time(); + count++; + + if ((unsigned long)(t2 - t1) < rpc_min) { + rpc_min = (unsigned long)(t2 - t1); + } + + if ((unsigned long)(t2 - t1) > rpc_max) { + rpc_max = (unsigned long)(t2 - t1); + } + + if ((unsigned long)(t3 - t2) < orb_min) { + orb_min = (unsigned long)(t3 - t2); + } + + if ((unsigned long)(t3 - t2) > orb_max) { + orb_max = (unsigned long)(t3 - t2); + } + + rpc_avg = ((double)((rpc_avg * (count - 1)) + (unsigned long)(t2 - t1))) / (double)(count); + orb_avg = ((double)((orb_avg * (count - 1)) + (unsigned long)(t3 - t2))) / (double)(count); + + if ((unsigned long)(t3 - check_time) >= 10000000) { + //PX4_DEBUG("Krait RPC Stats : rpc_min: %lu rpc_max: %lu rpc_avg: %f", rpc_min, rpc_max, rpc_avg); + //PX4_DEBUG("Krait RPC(orb) Stats: orb_min: %lu orb_max: %lu orb_avg: %f", orb_min, orb_max, orb_avg); + check_time = t3; + rpc_max = orb_max = 0; + rpc_min = orb_min = 0xFFFFFF; + orb_avg = 0; + rpc_avg = 0; + count = 0; + } + + //PX4_DEBUG("MsgName: %30s, t1: %lu, t2: %lu, t3: %lu, dt1: %lu, dt2: %lu",name, (unsigned long) t1, (unsigned long) t2, (unsigned long) t3, + // (unsigned long) (t2-t1), (unsigned long) (t3-t2)); + } + + PX4_DEBUG("[uORB::KraitFastRpcChannel::fastrpc_recv_thread] Exiting fastrpc_recv_thread\n"); +} + +void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics) +{ + FILE *fp = fopen(_log_file_name.c_str(), "a+"); + + if (fp == nullptr) { + PX4_ERR("Error unable to open log file[%s]", _log_file_name.c_str()); + return; + } + + fprintf(fp, "===== Data Len[%d] num_topics[%d] ======\n", length, num_topics); + + for (int i = 0; i < length; i += 16) { + int remaining_chars = length - i; + remaining_chars = (remaining_chars >= 16) ? 16 : remaining_chars; + + fprintf(fp, "%p - ", &(buffer[i])); + + for (int j = 0; j < remaining_chars; j++) { + fprintf(fp, " %02X", buffer[i + j]); + + if (j == 7) { + fprintf(fp, " -"); + } + } + + fprintf(fp, " "); + + for (int j = 0; j < remaining_chars; j++) { + fprintf(fp, "%c", (char)buffer[i + j ]); + } + + fprintf(fp, "\n"); + } + + fclose(fp); +} + diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp new file mode 100644 index 000000000000..0e8d82b44b64 --- /dev/null +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ + +#ifndef _uORBKraitFastRpcChannel_hpp_ +#define _uORBKraitFastRpcChannel_hpp_ + +#include +#include +#include +#include "uORB/uORBCommunicator.hpp" +#include "px4muorb_KraitRpcWrapper.hpp" +#include +#include "drivers/drv_hrt.h" + +namespace uORB +{ +class KraitFastRpcChannel; +} + +class uORB::KraitFastRpcChannel : public uORBCommunicator::IChannel +{ +public: + /** + * static method to get the IChannel Implementor. + */ + static uORB::KraitFastRpcChannel *GetInstance() + { + return &(_Instance); + } + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription(const char *messageName); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); + + + void Start(); + void Stop(); + +private: // data members + static uORB::KraitFastRpcChannel _Instance; + uORBCommunicator::IChannelRxHandler *_RxHandler; + pthread_t _RecvThread; + bool _ThreadStarted; + bool _ThreadShouldExit; + + static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; + static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; + static const int32_t _DATA_MSG_TYPE = 3; + + struct BulkTransferHeader { + uint16_t _MsgNameLen; + uint16_t _DataLen; + }; + + px4muorb::KraitRpcWrapper _KraitWrapper; + + std::map _AdspSubscriberCache; + std::map _AdspSubscriberSampleTimestamp; + //hrt_abstime _SubCacheSampleTimestamp; + static const hrt_abstime _SubCacheRefreshRate = 1000000; // 1 second; + +private://class members. + /// constructor. + KraitFastRpcChannel(); + + static void *thread_start(void *handler); + + void fastrpc_recv_thread(); + +}; + +#endif /* _uORBKraitFastRpcChannel_hpp_ */ diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt new file mode 100644 index 000000000000..d8399ccafd63 --- /dev/null +++ b/src/modules/navigator/CMakeLists.txt @@ -0,0 +1,56 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__navigator + MAIN navigator + STACK 1200 + COMPILE_FLAGS + -Wno-sign-compare + -Os + SRCS + navigator_main.cpp + navigator_mode.cpp + mission_block.cpp + mission.cpp + loiter.cpp + rtl.cpp + mission_feasibility_checker.cpp + geofence.cpp + datalinkloss.cpp + rcloss.cpp + enginefailure.cpp + gpsfailure.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 7925809f129c..9f26d0501681 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 5d7775bb23ff..9eb0299ab76f 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,13 +36,9 @@ * * Parameters for DLL * - * @author Thomas Gubler + * @author Thomas Gubler */ -#include - -#include - /* * Data Link Loss parameters, accessible via MAVLink */ @@ -64,7 +60,8 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); * Latitude of comms hold waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -900000000 + * @max 900000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); @@ -75,7 +72,8 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); * Longitude of comms hold waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -1800000000 + * @max 1800000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); @@ -86,13 +84,14 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); * Altitude of comms hold waypoint * * @unit m - * @min 0.0 + * @min -50 + * @max 30000 * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); /** - * Aifield hole wait time + * Airfield hole wait time * * The amount of time in seconds the system should wait at the airfield home waypoint * diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index b0130a1f5c14..0c344bff9277 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index a8f041856362..2baa0a758134 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -53,14 +53,8 @@ #include #include -#define GEOFENCE_OFF 0 -#define GEOFENCE_FILE_ONLY 1 -#define GEOFENCE_MAX_DISTANCES_ONLY 2 -#define GEOFENCE_FILE_AND_MAX_DISTANCES 3 - #define GEOFENCE_RANGE_WARNING_LIMIT 3000000 - /* Oddly, ERROR is not defined for C++ */ #ifdef ERROR # undef ERROR @@ -76,8 +70,8 @@ Geofence::Geofence() : _last_vertical_range_warning(0), _altitude_min(0), _altitude_max(0), - _verticesCount(0), - _param_geofence_mode(this, "MODE"), + _vertices_count(0), + _param_action(this, "ACTION"), _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), @@ -138,7 +132,6 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, bool Geofence::inside(double lat, double lon, float altitude) { - if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) { int32_t max_horizontal_distance = _param_max_hor_distance.get(); int32_t max_vertical_distance = _param_max_ver_distance.get(); @@ -152,7 +145,7 @@ bool Geofence::inside(double lat, double lon, float altitude) if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) { if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m", + mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m", (double)(dist_z - max_vertical_distance)); _last_vertical_range_warning = hrt_absolute_time(); } @@ -162,7 +155,7 @@ bool Geofence::inside(double lat, double lon, float altitude) if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) { if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m", + mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m", (double)(dist_xy - max_horizontal_distance)); _last_horizontal_range_warning = hrt_absolute_time(); } @@ -171,7 +164,6 @@ bool Geofence::inside(double lat, double lon, float altitude) } } } - } bool inside_fence = inside_polygon(lat, lon, altitude); @@ -194,12 +186,6 @@ 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 or only checking max distances */ - if ((_param_geofence_mode.get() == GEOFENCE_OFF) - || (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) { - return true; - } - if (valid()) { if (!isEmpty()) { @@ -219,7 +205,7 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude) struct fence_vertex_s temp_vertex_j; /* Red until fence is finished */ - for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) { + for (unsigned i = 0, j = _vertices_count - 1; i < _vertices_count; j = i++) { if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } @@ -259,7 +245,7 @@ Geofence::valid() } // Otherwise - if ((_verticesCount < 4) || (_verticesCount > fence_s::GEOFENCE_MAX_VERTICES)) { + if ((_vertices_count < 4) || (_vertices_count > fence_s::GEOFENCE_MAX_VERTICES)) { warnx("Fence must have at least 3 sides and not more than %d", fence_s::GEOFENCE_MAX_VERTICES - 1); return false; } @@ -282,13 +268,13 @@ Geofence::addPoint(int argc, char *argv[]) } if (argc < 3) { - errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + PX4_WARN("Specify: -clear | sequence latitude longitude [-publish]"); } ix = atoi(argv[0]); if (ix >= DM_KEY_FENCE_POINTS_MAX) { - errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + PX4_WARN("Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); } lat = strtod(argv[1], &end); @@ -311,7 +297,7 @@ Geofence::addPoint(int argc, char *argv[]) return; } - errx(1, "can't store fence point"); + PX4_WARN("can't store fence point"); } void @@ -415,7 +401,7 @@ Geofence::loadFromFile(const char *filename) /* Check if import was successful */ if (gotVertical && pointCounter > 0) { - _verticesCount = pointCounter; + _vertices_count = pointCounter; warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); rc = OK; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 96764cc976bb..a19e13ca2f86 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -49,8 +49,9 @@ #include #include #include +#include -#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" +#define GEOFENCE_FILENAME PX4_ROOTFSDIR"/fs/microsd/etc/geofence.txt" class Geofence : public control::SuperBlock { @@ -96,12 +97,14 @@ class Geofence : public control::SuperBlock int loadFromFile(const char *filename); - bool isEmpty() {return _verticesCount == 0;} + bool isEmpty() {return _vertices_count == 0;} int getAltitudeMode() { return _param_altitude_mode.get(); } int getSource() { return _param_source.get(); } + int getGeofenceAction() { return _param_action.get(); } + void setMavlinkFd(int value) { _mavlinkFd = value; } private: @@ -113,20 +116,20 @@ class Geofence : public control::SuperBlock hrt_abstime _last_horizontal_range_warning; hrt_abstime _last_vertical_range_warning; - float _altitude_min; - float _altitude_max; + float _altitude_min; + float _altitude_max; - unsigned _verticesCount; + uint8_t _vertices_count; /* Params */ - control::BlockParamInt _param_geofence_mode; + control::BlockParamInt _param_action; control::BlockParamInt _param_altitude_mode; control::BlockParamInt _param_source; control::BlockParamInt _param_counter_threshold; control::BlockParamInt _param_max_hor_distance; control::BlockParamInt _param_max_ver_distance; - uint8_t _outside_counter; + uint8_t _outside_counter; int _mavlinkFd; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 7c1d8a266a44..070fde617463 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -39,24 +39,20 @@ * @author Thomas Gubler */ -#include - -#include - /* * Geofence parameters, accessible via MAVLink */ /** - * Geofence mode. + * Geofence violation action. * - * 0 = disabled, 1 = geofence file only, 2 = max horizontal (GF_MAX_HOR_DIST) and vertical (GF_MAX_VER_DIST) distances, 3 = both + * 0 = none, 1 = warning (default), 2 = loiter, 3 = return to launch, 4 = fight termination * * @min 0 - * @max 3 + * @max 4 * @group Geofence */ -PARAM_DEFINE_INT32(GF_MODE, 0); +PARAM_DEFINE_INT32(GF_ACTION, 1); /** * Geofence altitude mode @@ -97,7 +93,7 @@ PARAM_DEFINE_INT32(GF_COUNT, -1); /** * Max horizontal distance in meters. * - * Set to > 0 to activate RTL if horizontal distance to home exceeds this value. + * Set to > 0 to activate a geofence action if horizontal distance to home exceeds this value. * * @group Geofence */ @@ -106,7 +102,7 @@ PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1); /** * Max vertical distance in meters. * - * Set to > 0 to activate RTL if vertical distance to home exceeds this value. + * Set to > 0 to activate a geofence action if vertical distance to home exceeds this value. * * @group Geofence */ diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 0ca69f0f75f0..e6097c87f350 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c index ca2f54050547..6c835d062e29 100644 --- a/src/modules/navigator/gpsfailure_params.c +++ b/src/modules/navigator/gpsfailure_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * GPS Failure Navigation Mode parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index a744d58cf084..aabdb2b075e1 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -55,7 +55,8 @@ #include "navigator.h" Loiter::Loiter(Navigator *navigator, const char *name) : - MissionBlock(navigator, name) + MissionBlock(navigator, name), + _param_min_alt(this, "MIS_TAKEOFF_ALT", false) { /* load initial params */ updateParams(); @@ -74,7 +75,7 @@ void Loiter::on_activation() { /* set current mission item to loiter */ - set_loiter_item(&_mission_item); + set_loiter_item(&_mission_item, _param_min_alt.get()); /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 37ab57a07822..0627c5412903 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -59,6 +59,9 @@ class Loiter : public MissionBlock virtual void on_activation(); virtual void on_active(); + +private: + control::BlockParamFloat _param_min_alt; }; #endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a68f4de012ed..e4068cac384e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -70,15 +71,15 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_dist_1wp(this, "MIS_DIST_1WP", false), _param_altmode(this, "MIS_ALTMODE", false), _param_yawmode(this, "MIS_YAWMODE", false), - _onboard_mission({0}), - _offboard_mission({0}), + _onboard_mission{}, + _offboard_mission{}, _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), _need_takeoff(true), _takeoff(false), _mission_type(MISSION_TYPE_NONE), _inited(false), - _dist_1wp_ok(false), + _home_inited(false), _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), _mission_item_previous_alt(NAN), @@ -111,13 +112,28 @@ Mission::on_inactive() } } else { - /* read mission topics on initialization */ - _inited = true; - update_onboard_mission(); - update_offboard_mission(); + /* load missions from storage */ + mission_s mission_state; + + dm_lock(DM_KEY_MISSION_STATE); + + /* read current state */ + int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + + dm_unlock(DM_KEY_MISSION_STATE); + + if (read_res == sizeof(mission_s)) { + _offboard_mission.dataman_id = mission_state.dataman_id; + _offboard_mission.count = mission_state.count; + _current_offboard_mission_index = mission_state.current_seq; + } + + _inited = true; } + check_mission_valid(); + /* require takeoff after non-loiter or landing */ if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) { _need_takeoff = true; @@ -133,6 +149,8 @@ Mission::on_activation() void Mission::on_active() { + check_mission_valid(); + /* check if anything has changed */ bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); @@ -176,6 +194,7 @@ Mission::on_active() && _mission_type != MISSION_TYPE_NONE) { heading_sp_update(); } + } void @@ -197,6 +216,11 @@ Mission::update_onboard_mission() /* otherwise, just leave it */ } + // XXX check validity here as well + _navigator->get_mission_result()->valid = true; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + } else { _onboard_mission.count = 0; _onboard_mission.current_seq = 0; @@ -230,18 +254,26 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt); + _navigator->get_home_position()->alt, _navigator->home_position_valid(), + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + + _navigator->get_mission_result()->valid = !failed; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); } else { - warnx("offboard mission update failed"); + PX4_WARN("offboard mission update failed, handle: %d", _navigator->get_offboard_mission_sub()); } if (failed) { _offboard_mission.count = 0; _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; + + warnx("mission check failed"); } set_current_offboard_mission_item(); @@ -271,70 +303,13 @@ Mission::advance_mission() } } -bool -Mission::check_dist_1wp() +int +Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item) { - if (_dist_1wp_ok) { - /* always return true after at least one successful check */ - return true; - } - - /* check if first waypoint is not too far from home */ - if (_param_dist_1wp.get() > 0.0f) { - if (_navigator->get_vstatus()->condition_home_position_valid) { - struct mission_item_s mission_item; - - /* find first waypoint (with lat/lon) item in datamanager */ - for (unsigned i = 0; i < _offboard_mission.count; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, - &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { - - /* check only items with valid lat/lon */ - if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || - mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || - mission_item.nav_cmd == NAV_CMD_TAKEOFF || - mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { - - /* check distance from home to item */ - float dist_to_1wp = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); - - if (dist_to_1wp < _param_dist_1wp.get()) { - _dist_1wp_ok = true; - if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) { - /* allow at 2/3 distance, but warn */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); - } - return true; - - } else { - /* item is too far from home */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get()); - return false; - } - } - - } else { - /* error reading, mission is invalid */ - mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission"); - return false; - } - } - - /* no waypoints found in mission, then we will not fly far away */ - _dist_1wp_ok = true; - return true; - - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), "no home position"); - return false; - } - + if (_mission_item.altitude_is_relative) { + return _mission_item.altitude + _navigator->get_home_position()->alt; } else { - return true; + return _mission_item.altitude; } } @@ -354,47 +329,45 @@ Mission::set_mission_items() /* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */ if (pos_sp_triplet->previous.valid) { - _mission_item_previous_alt = _mission_item.altitude; + _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item); } - /* get home distance state */ - bool home_dist_ok = check_dist_1wp(); /* the home dist check provides user feedback, so we initialize it to this */ - bool user_feedback_done = !home_dist_ok; + bool user_feedback_done = false; /* try setting onboard mission item */ if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); + user_feedback_done = true; } _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { + } else if (read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); + mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); + user_feedback_done = true; } _mission_type = MISSION_TYPE_OFFBOARD; } 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"); + /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + user_feedback_done = true; /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); - } else if (!user_feedback_done) { - /* only tell users that we got no mission if there has not been any - * better, more specific feedback yet - */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available"); } + _mission_type = MISSION_TYPE_NONE; - /* set loiter mission item */ - set_loiter_item(&_mission_item); + /* set loiter mission item and ensure that there is a minimum clearance from home */ + set_loiter_item(&_mission_item, _param_takeoff_alt.get()); /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; @@ -406,6 +379,24 @@ Mission::set_mission_items() set_mission_finished(); + if (!user_feedback_done) { + /* only tell users that we got no mission if there has not been any + * better, more specific feedback yet + * https://en.wikipedia.org/wiki/Loiter_(aeronautics) + */ + + if (_navigator->get_vstatus()->condition_landed) { + /* landed, refusing to take off without a mission */ + + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff"); + } else { + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering"); + } + + user_feedback_done = true; + + } + _navigator->set_position_setpoint_triplet_updated(); return; } @@ -440,10 +431,7 @@ Mission::set_mission_items() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next); /* calculate takeoff altitude */ - float takeoff_alt = _mission_item.altitude; - if (_mission_item.altitude_is_relative) { - takeoff_alt += _navigator->get_home_position()->alt; - } + float takeoff_alt = get_absolute_altitude_for_item(_mission_item); /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_vstatus()->condition_landed) { @@ -533,7 +521,7 @@ Mission::heading_sp_update() /* Don't change setpoint if last and current waypoint are not valid */ if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || - !isfinite(_on_arrival_yaw)) { + !PX4_ISFINITE(_on_arrival_yaw)) { return; } @@ -581,12 +569,12 @@ Mission::altitude_sp_foh_update() /* Don't change setpoint if last and current waypoint are not valid */ if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || - !isfinite(_mission_item_previous_alt)) { + !PX4_ISFINITE(_mission_item_previous_alt)) { return; } /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ - if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) { + if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < 0.0f) { return; } @@ -608,16 +596,16 @@ Mission::altitude_sp_foh_update() /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ - if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) { - pos_sp_triplet->current.alt = _mission_item.altitude; + if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { + pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item); } else { /* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp * of the mission item as it is used to check if the mission item is reached * The setpoint is set linearly and such that the system reaches the current altitude at the acceptance * radius around the current waypoint **/ - float delta_alt = (_mission_item.altitude - _mission_item_previous_alt); - float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius); + float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt); + float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius)); float a = _mission_item_previous_alt - grad * _distance_current_previous; pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; @@ -637,26 +625,29 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s { /* select onboard/offboard mission */ int *mission_index_ptr; - struct mission_s *mission; dm_item_t dm_item; - int mission_index_next; + + struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission; + int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index; + + /* do not work on empty missions */ + if (mission->count == 0) { + return false; + } + + /* move to next item if there is one */ + if (mission_index_next < ((int)mission->count - 1)) { + mission_index_next++; + } if (onboard) { /* onboard mission */ - mission_index_next = _current_onboard_mission_index + 1; mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; - - mission = &_onboard_mission; - dm_item = DM_KEY_WAYPOINTS_ONBOARD; } else { /* offboard mission */ - mission_index_next = _current_offboard_mission_index + 1; mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; - - mission = &_offboard_mission; - dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } @@ -666,6 +657,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { /* mission item index out of bounds */ + mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); return false; } @@ -677,7 +669,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - mavlink_log_critical(_navigator->get_mavlink_fd(), + mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "ERROR waypoint could not be read"); return false; } @@ -809,3 +801,26 @@ Mission::set_mission_finished() _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); } + +bool +Mission::check_mission_valid() +{ + /* check if the home position became valid in the meantime */ + if (!_home_inited && _navigator->home_position_valid()) { + + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); + + _navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, + dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), + _navigator->get_home_position()->alt, _navigator->home_position_valid(), + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + + _home_inited = true; + } + + return _navigator->get_mission_result()->valid; +} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index e9f78e8fdc61..f367da1c7d42 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,6 +39,7 @@ * @author Thomas Gubler * @author Anton Babushkin * @author Ban Siesta + * @author Lorenz Meier */ #ifndef NAVIGATOR_MISSION_H @@ -132,6 +133,8 @@ class Mission : public MissionBlock */ void altitude_sp_foh_reset(); + int get_absolute_altitude_for_item(struct mission_item_s &mission_item); + /** * Read current or next mission item from the dataman and watch out for DO_JUMPS * @return true if successful @@ -163,6 +166,11 @@ class Mission : public MissionBlock */ void set_mission_finished(); + /** + * Check wether a mission is ready to go + */ + bool check_mission_valid(); + control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; @@ -184,7 +192,7 @@ class Mission : public MissionBlock } _mission_type; bool _inited; - bool _dist_1wp_ok; + bool _home_inited; MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index dce7d4517112..94ded8a4b953 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -50,17 +50,23 @@ #include #include +#include #include "navigator.h" #include "mission_block.h" +actuator_controls_s actuators; +orb_advert_t actuator_pub_fd; + MissionBlock::MissionBlock(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), _mission_item({0}), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0) + _time_first_inside_orbit(0), + _actuators{}, + _actuator_pub(nullptr) { } @@ -71,6 +77,15 @@ MissionBlock::~MissionBlock() bool MissionBlock::is_mission_item_reached() { + if (_mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) { + actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_2), &actuators); + memset(&actuators, 0, sizeof(actuators)); + actuators.control[_mission_item.actuator_num] = 1.0f / 2000 * -_mission_item.actuator_value; + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_2), actuator_pub_fd, &actuators); + return true; + } + if (_mission_item.nav_cmd == NAV_CMD_IDLE) { return false; } @@ -80,8 +95,8 @@ MissionBlock::is_mission_item_reached() } /* TODO: count turns */ - if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { + if (/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { return false; } @@ -123,12 +138,12 @@ MissionBlock::is_mission_item_reached() * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ - if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) { _waypoint_position_reached = true; } } else { /* for normal mission items used their acceptance radius */ - if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { _waypoint_position_reached = true; } } @@ -138,7 +153,7 @@ MissionBlock::is_mission_item_reached() if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ - if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { + if (_navigator->get_vstatus()->is_rotary_wing && PX4_ISFINITE(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); @@ -193,6 +208,13 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->pitch_min = item->pitch_min; switch (item->nav_cmd) { + case NAV_CMD_DO_SET_SERVO: + /* Set current position for loitering set point*/ + sp->lat = _navigator->get_global_position()->lat; + sp->lon = _navigator->get_global_position()->lon; + sp->alt = _navigator->get_global_position()->alt; + sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; + break; case NAV_CMD_IDLE: sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE; break; @@ -228,7 +250,7 @@ MissionBlock::set_previous_pos_setpoint() } void -MissionBlock::set_loiter_item(struct mission_item_s *item) +MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) { if (_navigator->get_vstatus()->condition_landed) { /* landed, don't takeoff, but switch to IDLE mode */ @@ -246,10 +268,14 @@ MissionBlock::set_loiter_item(struct mission_item_s *item) item->altitude = pos_sp_triplet->current.alt; } else { - /* use current position */ + /* use current position and use return altitude as clearance */ item->lat = _navigator->get_global_position()->lat; item->lon = _navigator->get_global_position()->lon; item->altitude = _navigator->get_global_position()->alt; + + if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) { + item->altitude = _navigator->get_home_position()->alt + min_clearance; + } } item->altitude_is_relative = false; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index ec3e30582501..eb17a329ee32 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -43,9 +43,12 @@ #include +#include + #include #include #include +#include #include "navigator_mode.h" @@ -91,12 +94,16 @@ class MissionBlock : public NavigatorMode /** * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position */ - void set_loiter_item(struct mission_item_s *item); + void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f); mission_item_s _mission_item; bool _waypoint_position_reached; bool _waypoint_yaw_reached; hrt_abstime _time_first_inside_orbit; + + actuator_controls_s _actuators; + orb_advert_t _actuator_pub; + }; #endif diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index d9297f25bf99..880d4bd98eaf 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -51,66 +51,70 @@ #include #include -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false) +MissionFeasibilityChecker::MissionFeasibilityChecker() : + _mavlink_fd(-1), + _capabilities_sub(-1), + _initDone(false), + _dist_1wp_ok(false) { _nav_caps = {0}; } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, + dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, + float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued) { bool failed = false; + bool warned = false; /* Init if not done yet */ init(); - /* Open mavlink fd */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = mavlink_fd; + + // first check if we have a valid position + if (!home_valid /* can later use global / local pos for finer granularity */) { + failed = true; + warned = true; + mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock."); + } else { + failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); } // check if all mission item commands are supported - failed |= !checkMissionItemValidity(dm_current, nMissionItems); - + failed = failed || !checkMissionItemValidity(dm_current, nMissionItems); + failed = failed || !checkGeofence(dm_current, nMissionItems, geofence); + failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); + + if (isRotarywing) { + failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); + } else { + failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); + } - if (isRotarywing) - failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); - else - failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + if (!failed) { + mavlink_log_info(_mavlink_fd, "Mission checked and ready."); + } return !failed; } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { - - /* Perform checks and issue feedback to the user for all checks */ - bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); - - /* Mission is only marked as feasible if all checks return true */ - return (resGeofence && resHomeAltitude); + /* no custom rotary wing checks yet */ + return true; } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); -// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); /* Perform checks and issue feedback to the user for all checks */ bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); - bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); /* Mission is only marked as feasible if all checks return true */ - return (resLanding && resGeofence && resHomeAltitude); + return resLanding; } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -136,7 +140,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } -bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error) +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, + float home_alt, bool home_valid, bool &warning_issued, bool throw_error) { /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ for (size_t i = 0; i < nMissionItems; i++) { @@ -144,18 +149,25 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { + warning_issued = true; /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - if (throw_error) { - return false; - } else { - return true; - } + return false; + } + + /* always reject relative alt without home set */ + if (missionitem.altitude_is_relative && !home_valid) { + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + warning_issued = true; + return false; } /* calculate the global waypoint altitude */ float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; if (home_alt > wp_alt) { + + warning_issued = true; + if (throw_error) { mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return false; @@ -191,13 +203,13 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s missionitem.nav_cmd != NAV_CMD_TAKEOFF && missionitem.nav_cmd != NAV_CMD_ROI && missionitem.nav_cmd != NAV_CMD_PATHPLANNING && - missionitem.nav_cmd != NAV_CMD_DO_JUMP) { + missionitem.nav_cmd != NAV_CMD_DO_JUMP && + missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO) { mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1)); return false; } } - mavlink_log_info(_mavlink_fd, "Mission ready."); return true; } @@ -269,6 +281,83 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size return true; } +bool +MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued) +{ + if (_dist_1wp_ok) { + /* always return true after at least one successful check */ + return true; + } + + /* check if first waypoint is not too far from home */ + if (dist_first_wp > 0.0f) { + struct mission_item_s mission_item; + + /* find first waypoint (with lat/lon) item in datamanager */ + for (unsigned i = 0; i < nMissionItems; i++) { + if (dm_read(dm_current, i, + &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + /* Check non navigation item */ + if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO){ + + /* check actuator number */ + if (mission_item.actuator_num < 0 || mission_item.actuator_num > 5) { + mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 0..5", (int)mission_item.actuator_num); + warning_issued = true; + return false; + } + /* check actuator value */ + if (mission_item.actuator_value < -2000 || mission_item.actuator_value > 2000) { + mavlink_log_critical(_mavlink_fd, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.actuator_value); + warning_issued = true; + return false; + } + } + /* check only items with valid lat/lon */ + else if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + mission_item.nav_cmd == NAV_CMD_TAKEOFF || + mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { + + /* check distance from current position to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, curr_lat, curr_lon); + + if (dist_to_1wp < dist_first_wp) { + _dist_1wp_ok = true; + if (dist_to_1wp > ((dist_first_wp * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + warning_issued = true; + } + return true; + + } else { + /* item is too far from home */ + mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp); + warning_issued = true; + return false; + } + } + + } else { + /* error reading, mission is invalid */ + mavlink_log_info(_mavlink_fd, "error reading offboard mission"); + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + _dist_1wp_ok = true; + return true; + + } else { + return true; + } +} + void MissionFeasibilityChecker::updateNavigationCapabilities() { (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 94b6b292260d..4586f75a47a9 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -57,20 +57,22 @@ class MissionFeasibilityChecker struct navigation_capabilities_s _nav_caps; bool _initDone; + bool _dist_1wp_ok; void init(); /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); - bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); + bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); public: MissionFeasibilityChecker(); @@ -79,8 +81,9 @@ class MissionFeasibilityChecker /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt); + bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, + size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, + double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued); }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 8e501fd8ef63..1ef0c57e3ed3 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -39,10 +39,6 @@ * @author Julian Oes */ -#include - -#include - /* * Mission parameters, accessible via MAVLink */ @@ -56,7 +52,7 @@ * @unit meters * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); +PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); /** * Enable persistent onboard mission storage @@ -81,7 +77,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * @max 1000 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900); /** * Altitude setpoint mode @@ -94,7 +90,7 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); * @max 1 * @group Mission */ -PARAM_DEFINE_INT32(MIS_ALTMODE, 0); +PARAM_DEFINE_INT32(MIS_ALTMODE, 1); /** * Multirotor only. Yaw setpoint mode. diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h new file mode 100644 index 000000000000..d23a20c5ffb0 --- /dev/null +++ b/src/modules/navigator/navigation.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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 navigation.h + * Definition of a mission consisting of mission items. + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + */ + +#ifndef NAVIGATION_H_ +#define NAVIGATION_H_ + +#include +#include + +#define NUM_MISSIONS_SUPPORTED 256 + +/* compatible to mavlink MAV_CMD */ +enum NAV_CMD { + NAV_CMD_IDLE = 0, + NAV_CMD_WAYPOINT = 16, + NAV_CMD_LOITER_UNLIMITED = 17, + NAV_CMD_LOITER_TURN_COUNT = 18, + NAV_CMD_LOITER_TIME_LIMIT = 19, + NAV_CMD_RETURN_TO_LAUNCH = 20, + NAV_CMD_LAND = 21, + NAV_CMD_TAKEOFF = 22, + NAV_CMD_ROI = 80, + NAV_CMD_PATHPLANNING = 81, + NAV_CMD_DO_JUMP = 177, + NAV_CMD_DO_SET_SERVO=183, + NAV_CMD_DO_REPEAT_SERVO=184 + +}; + +enum ORIGIN { + ORIGIN_MAVLINK = 0, + ORIGIN_ONBOARD +}; + +/** + * @addtogroup topics + * @{ + */ + +/** + * Global position setpoint in WGS84 coordinates. + * + * This is the position the MAV is heading towards. If it of type loiter, + * the MAV is circling around it with the given loiter radius in meters. + */ +struct mission_item_s { + bool altitude_is_relative; /**< true if altitude is relative from start point */ + double lat; /**< latitude in degrees */ + double lon; /**< longitude in degrees */ + float altitude; /**< altitude in meters (AMSL) */ + float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ + float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ + int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ + enum NAV_CMD nav_cmd; /**< navigation command */ + float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ + float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ + float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ + bool autocontinue; /**< true if next waypoint should follow after this one */ + enum ORIGIN origin; /**< where the waypoint has been generated */ + int do_jump_mission_index; /**< index where the do jump will go to */ + unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */ + unsigned do_jump_current_count; /**< count how many times the jump has been done */ + int actuator_num; /**< actuator number to be set 0..5 ( corresponds to AUX outputs 1..6 */ + int actuator_value; /**< new value for selected actuator in ms 900...2000 */ +}; + +#include + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(offboard_mission); +ORB_DECLARE(onboard_mission); + +#endif diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d2acce7899e8..6ecf73a606df 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -45,6 +45,7 @@ #include #include +#include #include #include @@ -134,6 +135,7 @@ class Navigator : public control::SuperBlock struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } + bool home_position_valid() { return (_home_pos.timestamp > 0); } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } struct geofence_result_s* get_geofence_result() { return &_geofence_result; } @@ -144,9 +146,26 @@ class Navigator : public control::SuperBlock Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } - float get_acceptance_radius() { return _param_acceptance_radius.get(); } + + /** + * Get the acceptance radius + * + * @return the distance at which the next waypoint should be used + */ + float get_acceptance_radius(); + + /** + * Get the acceptance radius given the mission item preset radius + * + * @param mission_item_radius the radius to use in case the controller-derived radius is smaller + * + * @return the distance at which the next waypoint should be used + */ + float get_acceptance_radius(float mission_item_radius); int get_mavlink_fd() { return _mavlink_fd; } + void increment_mission_instance_count() { _mission_instance_count++; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -186,9 +205,8 @@ class Navigator : public control::SuperBlock geofence_result_s _geofence_result; vehicle_attitude_setpoint_s _att_sp; - bool _home_position_set; - bool _mission_item_valid; /**< flags if the current mission item is valid */ + int _mission_instance_count; /**< instance count for the current mission */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -237,7 +255,7 @@ class Navigator : public control::SuperBlock /** * Retrieve home position */ - void home_position_update(); + void home_position_update(bool force=false); /** * Retreive navigation capabilities diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6a98a7aab1d8..b056e3fd9991 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -44,6 +44,9 @@ */ #include +#include +#include +#include #include #include @@ -123,8 +126,8 @@ Navigator::Navigator() : _pos_sp_triplet{}, _mission_result{}, _att_sp{}, - _home_position_set(false), _mission_item_valid(false), + _mission_instance_count(0), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, _geofence_violation_warning_sent(false), @@ -174,7 +177,7 @@ Navigator::~Navigator() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_navigator_task); + px4_task_delete(_navigator_task); break; } } while (_navigator_task != -1); @@ -202,14 +205,13 @@ Navigator::sensor_combined_update() } void -Navigator::home_position_update() +Navigator::home_position_update(bool force) { bool updated = false; orb_check(_home_pos_sub, &updated); - if (updated) { + if (updated || force) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); - _home_position_set = true; } } @@ -254,7 +256,7 @@ Navigator::task_main_trampoline(int argc, char *argv[]) void Navigator::task_main() { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: @@ -291,7 +293,7 @@ Navigator::task_main() global_position_update(); gps_position_update(); sensor_combined_update(); - home_position_update(); + home_position_update(true); navigation_capabilities_update(); params_update(); @@ -303,7 +305,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[8]; + px4_pollfd_struct_t fds[8]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -325,16 +327,17 @@ Navigator::task_main() while (!_task_should_exit) { - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + /* wait for up to 200ms for data */ + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ + PX4_WARN("timed out"); continue; } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ - warn("poll error %d, %d", pret, errno); + PX4_WARN("poll error %d, %d", pret, errno); continue; } @@ -343,7 +346,7 @@ Navigator::task_main() if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { /* try to reopen the mavlink log device with specified interval */ mavlink_open_time = hrt_abstime() + mavlink_open_interval; - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); } static bool have_geofence_position_data = false; @@ -397,10 +400,14 @@ Navigator::task_main() /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; - if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set); + if (have_geofence_position_data && + (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && + (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; + + _geofence_result.geofence_action = _geofence.getGeofenceAction(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; @@ -433,8 +440,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_mission; + if (_nav_caps.abort_landing) { + // pos controller aborted landing, requests loiter + // above landing waypoint + _navigation_mode = &_loiter; + _pos_sp_triplet_published_invalid_once = false; + } else { + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_mission; + } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; @@ -477,7 +491,7 @@ Navigator::task_main() } /* iterate through navigation modes and set active/inactive for each */ - for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { + for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } @@ -505,7 +519,7 @@ Navigator::task_main() warnx("exiting."); _navigator_task = -1; - _exit(0); + return; } int @@ -516,9 +530,9 @@ Navigator::start() /* start the task */ _navigator_task = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT + 20, - 1700, - (main_t)&Navigator::task_main_trampoline, + SCHED_PRIORITY_DEFAULT + 5, + 1500, + (px4_main_t)&Navigator::task_main_trampoline, nullptr); if (_navigator_task < 0) { @@ -571,6 +585,26 @@ Navigator::publish_position_setpoint_triplet() } } +float +Navigator::get_acceptance_radius() +{ + return get_acceptance_radius(_param_acceptance_radius.get()); +} + +float +Navigator::get_acceptance_radius(float mission_item_radius) +{ + float radius = mission_item_radius; + + if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { + if (_nav_caps.turn_distance > radius) { + radius = _nav_caps.turn_distance; + } + } + + return radius; +} + void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); @@ -584,54 +618,57 @@ void Navigator::load_fence_from_file(const char *filename) static void usage() { - errx(1, "usage: navigator {start|stop|status|fence|fencefile}"); + warnx("usage: navigator {start|stop|status|fence|fencefile}"); } int navigator_main(int argc, char *argv[]) { if (argc < 2) { usage(); + return 1; } if (!strcmp(argv[1], "start")) { if (navigator::g_navigator != nullptr) { - errx(1, "already running"); + warnx("already running"); + return 1; } navigator::g_navigator = new Navigator; if (navigator::g_navigator == nullptr) { - errx(1, "alloc failed"); + warnx("alloc failed"); + return 1; } if (OK != navigator::g_navigator->start()) { delete navigator::g_navigator; navigator::g_navigator = nullptr; - err(1, "start failed"); + warnx("start failed"); + return 1; } return 0; } - if (navigator::g_navigator == nullptr) - errx(1, "not running"); + if (navigator::g_navigator == nullptr) { + warnx("not running"); + return 1; + } if (!strcmp(argv[1], "stop")) { delete navigator::g_navigator; navigator::g_navigator = nullptr; - } else if (!strcmp(argv[1], "status")) { navigator::g_navigator->status(); - } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); - } else if (!strcmp(argv[1], "fencefile")) { navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); - } else { usage(); + return 1; } return 0; @@ -640,6 +677,8 @@ int navigator_main(int argc, char *argv[]) void Navigator::publish_mission_result() { + _mission_result.instance_count = _mission_instance_count; + /* lazily publish the mission result only once available */ if (_mission_result_pub != nullptr) { /* publish mission result */ @@ -656,6 +695,7 @@ Navigator::publish_mission_result() _mission_result.item_do_jump_changed = false; _mission_result.item_changed_index = 0; _mission_result.item_do_jump_remaining = 0; + _mission_result.valid = true; } void diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index b4f948fff0b0..17e580c6645f 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,22 +36,18 @@ * * Parameters for navigator in general * - * @author Julian Oes - * @author Thomas Gubler + * @author Julian Oes + * @author Thomas Gubler */ -#include - -#include - /** * Loiter radius (FW only) * * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). * - * @unit meters - * @min 20 - * @max 200 + * @unit meter + * @min 25 + * @max 1000 * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); @@ -61,9 +57,9 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); * * Default acceptance radius, overridden by acceptance radius of waypoint if set. * - * @unit meters + * @unit meter * @min 0.05 - * @max 200 + * @max 200.0 * @group Mission */ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f); @@ -74,6 +70,7 @@ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f); * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules * * @min 0 + * @max 1 * @group Mission */ PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); @@ -84,6 +81,7 @@ PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules * * @min 0 + * @max 1 * @group Mission */ PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); @@ -94,7 +92,8 @@ PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); * Latitude of airfield home waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -900000000 + * @max 900000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); @@ -105,7 +104,8 @@ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); * Longitude of airfield home waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -1800000000 + * @max 1800000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); @@ -116,7 +116,7 @@ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); * Altitude of airfield home waypoint * * @unit m - * @min 0.0 + * @min -50 * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f); diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 40bf7f022300..fcc00148f808 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -47,7 +47,7 @@ #include #include -#include +#include #include #include "navigator.h" diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c index 3c9a6e40ef5f..958dab9266df 100644 --- a/src/modules/navigator/rcloss_params.c +++ b/src/modules/navigator/rcloss_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * OBC RC Loss mode parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b75b7fa2214b..a77f5895e108 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -47,7 +47,7 @@ #include #include -#include +#include #include #include "navigator.h" @@ -58,6 +58,7 @@ RTL::RTL(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _rtl_state(RTL_STATE_NONE), + _rtl_start_lock(false), _param_return_alt(this, "RTL_RETURN_ALT", false), _param_descend_alt(this, "RTL_DESCEND_ALT", false), _param_land_delay(this, "RTL_LAND_DELAY", false) @@ -95,6 +96,7 @@ RTL::on_activation() } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) { _rtl_state = RTL_STATE_CLIMB; + _rtl_start_lock = false; /* otherwise go straight to return */ } else { @@ -102,6 +104,7 @@ RTL::on_activation() _rtl_state = RTL_STATE_RETURN; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_global_position()->alt; + _rtl_start_lock = false; } } @@ -126,7 +129,10 @@ RTL::set_rtl_item() /* make sure we have the latest params */ updateParams(); - set_previous_pos_setpoint(); + if (!_rtl_start_lock) { + set_previous_pos_setpoint(); + } + _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { @@ -182,6 +188,8 @@ RTL::set_rtl_item() mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + + _rtl_start_lock = true; break; } @@ -190,7 +198,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); - _mission_item.yaw = NAN; + _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; @@ -213,7 +221,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); - _mission_item.yaw = NAN; + _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; @@ -239,7 +247,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt; - _mission_item.yaw = NAN; + _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LAND; @@ -258,6 +266,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt; + // Do not change / control yaw in landed _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 5928f1f07b7a..464a1d4ee441 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -44,8 +44,7 @@ #include #include -#include -#include +#include #include #include @@ -88,6 +87,8 @@ class RTL : public MissionBlock RTL_STATE_LANDED, } _rtl_state; + bool _rtl_start_lock; + control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index a33ded28ab83..e65a41b775c9 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -39,10 +39,6 @@ * @author Julian Oes */ -#include - -#include - /* * RTL parameters, accessible via MAVLink */ diff --git a/src/modules/param/CMakeLists.txt b/src/modules/param/CMakeLists.txt new file mode 100644 index 000000000000..64bff2a0e189 --- /dev/null +++ b/src/modules/param/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include_directories(${CMAKE_CURRENT_BINARY_DIR}) + +px4_generate_parameters_source(OUT param_files + XML ${CMAKE_BINARY_DIR}/parameters.xml + DEPS xml_gen + ) + +px4_add_module( + MODULE modules__param + COMPILE_FLAGS + -Os + SRCS ${param_files} + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt new file mode 100644 index 000000000000..60429f5d2212 --- /dev/null +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if(${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000) +endif() +px4_add_module( + MODULE modules__position_estimator_inav + MAIN position_estimator_inav + STACK 1200 + COMPILE_FLAGS ${MODULE_CFLAGS} + SRCS + position_estimator_inav_main.c + position_estimator_inav_params.c + inertial_filter.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : 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 8654a7cb1100..d8f29f10828d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,8 +36,9 @@ * Model-identification based position estimator for multirotors * * @author Anton Babushkin + * @author Nuno Marques */ - +#include #include #include #include @@ -45,9 +46,6 @@ #include #include #include -#include -#include -#include #include #include #include @@ -64,8 +62,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -84,15 +84,15 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ -static bool verbose_mode = false; +static bool inav_verbose_mode = false; static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout = 0.5s +static const hrt_abstime mocap_topic_timeout = 500000; // Mocap topic timeout = 0.5s static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s -static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms -static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss -static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss -static const uint32_t updates_counter_len = 1000000; +static const hrt_abstime lidar_timeout = 150000; // lidar timeout = 150ms +static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distance during this time after lidar loss +static const unsigned updates_counter_len = 1000000; static const float max_flow = 1.0f; // max flow value that can be used, rad/s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -116,12 +116,12 @@ static inline int max(int val1, int val2) */ static void usage(const char *reason) { - if (reason) { - fprintf(stderr, "%s\n", reason); + if (reason && *reason) { + PX4_INFO("%s", reason); } - fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); - exit(1); + PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n"); + return; } /** @@ -142,22 +142,21 @@ int position_estimator_inav_main(int argc, char *argv[]) if (thread_running) { warnx("already running"); /* this is not an error */ - exit(0); + return 0; } - verbose_mode = false; + inav_verbose_mode = false; - if (argc > 1) - if (!strcmp(argv[2], "-v")) { - verbose_mode = true; - } + if ((argc > 2) && (!strcmp(argv[2], "-v"))) { + inav_verbose_mode = true; + } thread_should_exit = false; position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, - (argv) ? (char * const *) &argv[2] : (char * const *) NULL); - exit(0); + (argv && argc > 2) ? (char * const *) &argv[2] : (char * const *) NULL); + return 0; } if (!strcmp(argv[1], "stop")) { @@ -169,7 +168,7 @@ int position_estimator_inav_main(int argc, char *argv[]) warnx("not started"); } - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { @@ -180,28 +179,35 @@ int position_estimator_inav_main(int argc, char *argv[]) warnx("not started"); } - exit(0); + return 0; } usage("unrecognized command"); - exit(1); + return 1; } -static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +#ifdef INAV_DEBUG +static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], + float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v, float corr_mocap[3][1], float w_mocap_p, + float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v) { - FILE *f = fopen("/fs/microsd/inav.log", "a"); + FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", - hrt_absolute_time(), msg, (double)dt, + (unsigned long long)hrt_absolute_time(), msg, (double)dt, (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1], (double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]); fwrite(s, 1, n, f); - n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", + n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f mocap_pos_corr=[%.5f %.5f %.5f] w_mocap_p=%.5f\n", (double)acc[0], (double)acc[1], (double)acc[2], (double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1], - (double)w_xy_gps_p, (double)w_xy_gps_v); + (double)w_xy_gps_p, (double)w_xy_gps_v, (double)corr_mocap[0][0], (double)corr_mocap[1][0], (double)corr_mocap[2][0], (double)w_mocap_p); + fwrite(s, 1, n, f); + n = snprintf(s, 256, "\tvision_pos_corr=[%.5f %.5f %.5f] vision_vel_corr=[%.5f %.5f %.5f] w_xy_vision_p=%.5f w_z_vision_p=%.5f w_xy_vision_v=%.5f\n", + (double)corr_vision[0][0], (double)corr_vision[1][0], (double)corr_vision[2][0], (double)corr_vision[0][1], (double)corr_vision[1][1], (double)corr_vision[2][1], + (double)w_xy_vision_p, (double)w_z_vision_p, (double)w_xy_vision_v); fwrite(s, 1, n, f); free(s); } @@ -209,6 +215,9 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e fsync(fileno(f)); fclose(f); } +#else +#define write_debug_log(...) +#endif /**************************************************************************** * main @@ -216,7 +225,7 @@ 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[]) { int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel @@ -241,6 +250,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float eph_vision = 0.2f; float epv_vision = 0.2f; + float eph_mocap = 0.05f; + float epv_mocap = 0.05f; + float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); @@ -267,6 +279,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) uint16_t gps_updates = 0; uint16_t attitude_updates = 0; uint16_t flow_updates = 0; + uint16_t vision_updates = 0; + uint16_t mocap_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); hrt_abstime pub_last = hrt_absolute_time(); @@ -291,22 +305,36 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { 0.0f, 0.0f }, // D (pos, vel) }; - float corr_sonar = 0.0f; - float corr_sonar_filtered = 0.0f; + float corr_mocap[3][1] = { + { 0.0f }, // N (pos) + { 0.0f }, // E (pos) + { 0.0f }, // D (pos) + }; + + float corr_lidar = 0.0f; + float corr_lidar_filtered = 0.0f; float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; - float sonar_prev = 0.0f; + float lidar_prev = 0.0f; //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) + hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered) + hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered) + + int n_flow = 0; + float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f }; + float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f }; + float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; + float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; + float yaw_comp[] = { 0.0f, 0.0f }; bool gps_valid = false; // GPS is valid - bool sonar_valid = false; // sonar is valid + bool lidar_valid = false; // lidar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) - bool vision_valid = false; + bool vision_valid = false; // vision is valid + bool mocap_valid = false; // mocap is valid /* declare and safely initialize all structs */ struct actuator_controls_s actuator; @@ -327,8 +355,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&flow, 0, sizeof(flow)); struct vision_position_estimate_s vision; memset(&vision, 0, sizeof(vision)); + struct att_pos_mocap_s mocap; + memset(&mocap, 0, sizeof(mocap)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); + struct distance_sensor_s lidar; + memset(&lidar, 0, sizeof(lidar)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -339,7 +371,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); + int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); + int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor)); /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); @@ -348,15 +382,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ - parameters_init(&pos_inav_param_handles); + inav_parameters_init(&pos_inav_param_handles); /* first parameters read at start up */ struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ /* first parameters update */ - parameters_update(&pos_inav_param_handles, ¶ms); + inav_parameters_update(&pos_inav_param_handles, ¶ms); - struct pollfd fds_init[1] = { + px4_pollfd_struct_t fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, }; @@ -366,7 +400,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) thread_running = true; while (wait_baro && !thread_should_exit) { - int ret = poll(fds_init, 1, 1000); + int ret = px4_poll(fds_init, 1, 1000); if (ret < 0) { /* poll error */ @@ -376,13 +410,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_timestamp != baro_timestamp) { - baro_timestamp = sensor.baro_timestamp; + if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) { + baro_timestamp = sensor.baro_timestamp[0]; /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - if (isfinite(sensor.baro_alt_meter)) { - baro_offset += sensor.baro_alt_meter; + if (PX4_ISFINITE(sensor.baro_alt_meter[0])) { + baro_offset += sensor.baro_alt_meter[0]; baro_init_cnt++; } @@ -400,12 +434,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* main loop */ - struct pollfd fds[1] = { + px4_pollfd_struct_t fds[1] = { { .fd = vehicle_attitude_sub, .events = POLLIN }, }; while (!thread_should_exit) { - int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate + int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -421,6 +455,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) attitude_updates++; bool updated; + bool updated2; /* parameter update */ orb_check(parameter_update_sub, &updated); @@ -428,7 +463,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - parameters_update(&pos_inav_param_handles, ¶ms); + inav_parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ @@ -451,7 +486,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_timestamp != accel_timestamp) { + if (sensor.accelerometer_timestamp[0] != accel_timestamp) { if (att.R_valid) { /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; @@ -473,69 +508,73 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(acc, 0, sizeof(acc)); } - accel_timestamp = sensor.accelerometer_timestamp; + accel_timestamp = sensor.accelerometer_timestamp[0]; accel_updates++; } - if (sensor.baro_timestamp != baro_timestamp) { - corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; - baro_timestamp = sensor.baro_timestamp; + if (sensor.baro_timestamp[0] != baro_timestamp) { + corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0]; + baro_timestamp = sensor.baro_timestamp[0]; baro_updates++; } } /* optical flow */ orb_check(optical_flow_sub, &updated); + orb_check(distance_sensor_sub, &updated2); - if (updated) { + if (updated && updated2) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); /* 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; - if ((flow.ground_distance_m > 0.31f) && - (flow.ground_distance_m < 4.0f) && - (PX4_R(att.R, 2, 2) > 0.7f) && - (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { + if ((lidar.current_distance > 0.21f) && + (lidar.current_distance < 4.0f) && + /*(PX4_R(att.R, 2, 2) > 0.7f) &&*/ + (fabsf(lidar.current_distance - lidar_prev) > FLT_EPSILON)) { - sonar_time = t; - sonar_prev = flow.ground_distance_m; - corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; - corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt; + lidar_time = t; + lidar_prev = lidar.current_distance; + corr_lidar = lidar.current_distance + surface_offset + z_est[0]; + corr_lidar_filtered += (corr_lidar - corr_lidar_filtered) * params.lidar_filt; - if (fabsf(corr_sonar) > params.sonar_err) { + if (fabsf(corr_lidar) > params.lidar_err) { /* correction is too large: spike or new ground level? */ - if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) { + if (fabsf(corr_lidar - corr_lidar_filtered) > params.lidar_err) { /* spike detected, ignore */ - corr_sonar = 0.0f; - sonar_valid = false; + corr_lidar = 0.0f; + lidar_valid = false; } else { /* new ground level */ - surface_offset -= corr_sonar; + surface_offset -= corr_lidar; surface_offset_rate = 0.0f; - corr_sonar = 0.0f; - corr_sonar_filtered = 0.0f; - sonar_valid_time = t; - sonar_valid = true; + corr_lidar = 0.0f; + corr_lidar_filtered = 0.0f; + lidar_valid_time = t; + lidar_valid = true; local_pos.surface_bottom_timestamp = t; mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { /* correction is ok, use it */ - sonar_valid_time = t; - sonar_valid = true; + lidar_valid_time = t; + lidar_valid = true; } } float flow_q = flow.quality / 255.0f; - float dist_bottom = - z_est[0] - surface_offset; + float dist_bottom = - z_est[0] - surface_offset; //lidar.current_distance; - if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) { + if (dist_bottom > 0.21f && flow_q > params.flow_q_min) { /* distance to surface */ - float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); + //float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar + float flow_dist = dist_bottom; //use this if using lidar + /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; @@ -548,11 +587,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; + + /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ + flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f; + flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f; + flow_gyrospeed[2] = flow.gyro_z_rate_integral/(float)flow.integration_timespan*1000000.0f; + + //moving average + if (n_flow >= 100) { + gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; + gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; + gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2]; + n_flow = 0; + flow_gyrospeed_filtered[0] = 0.0f; + flow_gyrospeed_filtered[1] = 0.0f; + flow_gyrospeed_filtered[2] = 0.0f; + att_gyrospeed_filtered[0] = 0.0f; + att_gyrospeed_filtered[1] = 0.0f; + att_gyrospeed_filtered[2] = 0.0f; + } else { + flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); + flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); + flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1); + att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); + att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); + att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1); + n_flow++; + } + + + /*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/ + yaw_comp[0] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]); + yaw_comp[1] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]); + + /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - //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; + //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) + flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0] - yaw_comp[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1] - yaw_comp[1];//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; @@ -575,6 +648,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); + /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) { @@ -699,9 +773,36 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_vision[2][1] = 0.0f - z_est[1]; } + vision_updates++; } } + /* vehicle mocap position */ + orb_check(att_pos_mocap_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap); + + /* reset position estimate on first mocap update */ + if (!mocap_valid) { + x_est[0] = mocap.x; + y_est[0] = mocap.y; + z_est[0] = mocap.z; + + mocap_valid = true; + + warnx("MOCAP data valid"); + mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid"); + } + + /* calculate correction for position */ + corr_mocap[0][0] = mocap.x - x_est[0]; + corr_mocap[1][0] = mocap.y - y_est[0]; + corr_mocap[2][0] = mocap.z - z_est[0]; + + mocap_updates++; + } + /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); @@ -737,7 +838,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - + /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; @@ -811,9 +912,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* check for timeout on FLOW topic */ - if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) { + if ((flow_valid || lidar_valid) && t > flow.timestamp + flow_topic_timeout) { flow_valid = false; - sonar_valid = false; + lidar_valid = false; warnx("FLOW timeout"); mavlink_log_info(mavlink_fd, "[inav] FLOW timeout"); } @@ -832,20 +933,33 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); } - /* check for sonar measurement timeout */ - if (sonar_valid && (t > (sonar_time + sonar_timeout))) { - corr_sonar = 0.0f; - sonar_valid = false; + /* check for timeout on mocap topic */ + if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) { + mocap_valid = false; + warnx("MOCAP timeout"); + mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout"); + } + + /* check for lidar measurement timeout */ + if (lidar_valid && (t > (lidar_time + lidar_timeout))) { + corr_lidar = 0.0f; + lidar_valid = false; } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; - dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms + dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 ms t_prev = t; /* increase EPH/EPV on each step */ + if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0 + eph = 0.001; + } if (eph < max_eph_epv) { eph *= 1.0f + dt; } + if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0 + epv = 0.001; + } if (epv < max_eph_epv) { epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } @@ -856,21 +970,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* use VISION if it's valid and has a valid weight parameter */ bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; + /* use MOCAP if it's valid and has a valid weight parameter */ + bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W; + if(params.disable_mocap) { //disable mocap if fake gps is used + use_mocap = false; + } /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy; + + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; - bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); + bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); if (dist_bottom_valid) { /* surface distance prediction */ surface_offset += surface_offset_rate * dt; /* surface distance correction */ - if (sonar_valid) { - surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt; - surface_offset -= corr_sonar * params.w_z_sonar * dt; + if (lidar_valid) { + surface_offset_rate -= corr_lidar * 0.5f * params.w_z_lidar * params.w_z_lidar * dt; + surface_offset -= corr_lidar * params.w_z_lidar * dt; } } @@ -883,6 +1003,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_vision_v = params.w_xy_vision_v; float w_z_vision_p = params.w_z_vision_p; + float w_mocap_p = params.w_mocap_p; + /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_xy_gps_p *= params.w_gps_flow; @@ -940,6 +1062,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; } + /* accelerometer bias correction for MOCAP (use buffered rotation matrix) */ + accel_bias_corr[0] = 0.0f; + accel_bias_corr[1] = 0.0f; + accel_bias_corr[2] = 0.0f; + + if (use_mocap) { + accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p; + accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p; + accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p; + } + /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; @@ -982,7 +1115,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, z_est, acc[2]); if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, + acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, + corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); } @@ -1001,11 +1136,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); } + if (use_mocap) { + epv = fminf(epv, epv_mocap); + inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); + } + if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, + acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, + corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); + memset(corr_mocap, 0, sizeof(corr_mocap)); corr_baro = 0; } else { @@ -1018,7 +1161,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, y_est, acc[1]); if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, + acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, + corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } @@ -1055,12 +1200,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + if (use_mocap) { + eph = fminf(eph, eph_mocap); + + inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p); + inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); + } + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, + acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, + corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); + memset(corr_mocap, 0, sizeof(corr_mocap)); memset(corr_flow, 0, sizeof(corr_flow)); } else { @@ -1073,23 +1228,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - if (verbose_mode) { + if (inav_verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( - "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", + "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s", (double)(accel_updates / updates_dt), (double)(baro_updates / updates_dt), (double)(gps_updates / updates_dt), (double)(attitude_updates / updates_dt), - (double)(flow_updates / updates_dt)); + (double)(flow_updates / updates_dt), + (double)(vision_updates / updates_dt), + (double)(mocap_updates / updates_dt)); updates_counter_start = t; accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; flow_updates = 0; + vision_updates = 0; + mocap_updates = 0; } } @@ -1112,6 +1271,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) buf_ptr = 0; } + /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; @@ -1130,7 +1290,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; - local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; + local_pos.dist_bottom_rate = - z_est[1] - surface_offset_rate; } local_pos.timestamp = t; 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 382e9e46d727..f303abaf6dde 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -86,15 +86,15 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f); /** - * Z axis weight for sonar + * Z axis weight for lidar * - * Weight (cutoff frequency) for sonar measurements. + * Weight (cutoff frequency) for lidar measurements. * * @min 0.0 * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_LIDAR, 3.0f); /** * XY axis weight for GPS position @@ -140,6 +140,18 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f); */ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); +/** + * Weight for mocap system + * + * Weight (cutoff frequency) for mocap position measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ + + PARAM_DEFINE_FLOAT(INAV_W_MOC_P, 10.0f); + /** * XY axis weight for optical flow * @@ -149,7 +161,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 9.0f); /** * XY axis weight for resetting velocity @@ -205,18 +217,18 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); * @max 1.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); +PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.3f); /** - * Weight for sonar filter + * Weight for lidar filter * - * Sonar filter detects spikes on sonar measurements and used to detect new surface level. + * Lidar filter detects spikes on lidar measurements and used to detect new surface level. * * @min 0.0 * @max 1.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); +PARAM_DEFINE_FLOAT(INAV_LIDAR_FILT, 0.05f); /** * Sonar maximal error for new surface @@ -228,7 +240,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); * @unit m * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); +PARAM_DEFINE_FLOAT(INAV_LIDAR_ERR, 0.5f); /** * Land detector time @@ -277,6 +289,41 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); */ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); +/** + * Flow module offset (center of rotation) in X direction + * + * Yaw X flow compensation + * + * @min -1.0 + * @max 1.0 + * @unit m + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f); + +/** + * Flow module offset (center of rotation) in Y direction + * + * Yaw Y flow compensation + * + * @min -1.0 + * @max 1.0 + * @unit m + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f); + +/** + * Disable mocap (set 0 if using fake gps) + * + * Disable mocap + * + * @min 0 + * @max 1 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0); + /** * Disable vision input * @@ -301,57 +348,65 @@ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); */ PARAM_DEFINE_INT32(INAV_ENABLED, 1); -int parameters_init(struct position_estimator_inav_param_handles *h) +int inav_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_z_lidar = param_find("INAV_W_Z_LIDAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P"); h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V"); + h->w_mocap_p = param_find("INAV_W_MOC_P"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); h->flow_q_min = param_find("INAV_FLOW_Q_MIN"); - h->sonar_filt = param_find("INAV_SONAR_FILT"); - h->sonar_err = param_find("INAV_SONAR_ERR"); + h->lidar_filt = param_find("INAV_LIDAR_FILT"); + h->lidar_err = param_find("INAV_LIDAR_ERR"); h->land_t = param_find("INAV_LAND_T"); h->land_disp = param_find("INAV_LAND_DISP"); h->land_thr = param_find("INAV_LAND_THR"); h->no_vision = param_find("CBRK_NO_VISION"); h->delay_gps = param_find("INAV_DELAY_GPS"); + h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X"); + h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y"); + h->disable_mocap = param_find("INAV_DISAB_MOCAP"); - return OK; + return 0; } -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) +int inav_parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); param_get(h->w_z_vision_p, &(p->w_z_vision_p)); - param_get(h->w_z_sonar, &(p->w_z_sonar)); + param_get(h->w_z_lidar, &(p->w_z_lidar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); param_get(h->w_xy_vision_v, &(p->w_xy_vision_v)); + param_get(h->w_mocap_p, &(p->w_mocap_p)); param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); param_get(h->flow_q_min, &(p->flow_q_min)); - param_get(h->sonar_filt, &(p->sonar_filt)); - param_get(h->sonar_err, &(p->sonar_err)); + param_get(h->lidar_filt, &(p->lidar_filt)); + param_get(h->lidar_err, &(p->lidar_err)); param_get(h->land_t, &(p->land_t)); param_get(h->land_disp, &(p->land_disp)); param_get(h->land_thr, &(p->land_thr)); param_get(h->no_vision, &(p->no_vision)); param_get(h->delay_gps, &(p->delay_gps)); + param_get(h->flow_module_offset_x, &(p->flow_module_offset_x)); + param_get(h->flow_module_offset_y, &(p->flow_module_offset_y)); + param_get(h->disable_mocap, &(p->disable_mocap)); - return OK; + return 0; } 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 51bbda412acf..07b78104bf1c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -46,24 +46,28 @@ struct position_estimator_inav_params { float w_z_gps_p; float w_z_gps_v; float w_z_vision_p; - float w_z_sonar; + float w_z_lidar; float w_xy_gps_p; float w_xy_gps_v; float w_xy_vision_p; float w_xy_vision_v; + float w_mocap_p; float w_xy_flow; float w_xy_res_v; float w_gps_flow; float w_acc_bias; float flow_k; float flow_q_min; - float sonar_filt; - float sonar_err; + float lidar_filt; + float lidar_err; float land_t; float land_disp; float land_thr; int32_t no_vision; float delay_gps; + float flow_module_offset_x; + float flow_module_offset_y; + int32_t disable_mocap; }; struct position_estimator_inav_param_handles { @@ -71,24 +75,28 @@ struct position_estimator_inav_param_handles { 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_z_lidar; param_t w_xy_gps_p; param_t w_xy_gps_v; param_t w_xy_vision_p; param_t w_xy_vision_v; + param_t w_mocap_p; param_t w_xy_flow; param_t w_xy_res_v; param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; param_t flow_q_min; - param_t sonar_filt; - param_t sonar_err; + param_t lidar_filt; + param_t lidar_err; param_t land_t; param_t land_disp; param_t land_thr; param_t no_vision; param_t delay_gps; + param_t flow_module_offset_x; + param_t flow_module_offset_y; + param_t disable_mocap; }; #define CBRK_NO_VISION_KEY 328754 @@ -97,10 +105,10 @@ struct position_estimator_inav_param_handles { * Initialize all parameter handles and values * */ -int parameters_init(struct position_estimator_inav_param_handles *h); +int inav_parameters_init(struct position_estimator_inav_param_handles *h); /** * Update all parameters * */ -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p); +int inav_parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p); diff --git a/src/modules/sdlog2/CMakeLists.txt b/src/modules/sdlog2/CMakeLists.txt new file mode 100644 index 000000000000..b438f30fd5ee --- /dev/null +++ b/src/modules/sdlog2/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if (${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=1500) +endif() +px4_add_module( + MODULE modules__sdlog2 + MAIN sdlog2 + PRIORITY "SCHED_PRIORITY_MAX-30" + STACK 1200 + COMPILE_FLAGS + ${MODULE_CFLAGS} + -Os + SRCS + sdlog2.c + logbuffer.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 9e25dd2f239e..2768994dded5 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -50,8 +50,8 @@ int logbuffer_init(struct logbuffer_s *lb, int size) lb->size = size; lb->write_ptr = 0; lb->read_ptr = 0; - lb->data = malloc(lb->size); - return (lb->data == 0) ? PX4_ERROR : PX4_OK; + lb->data = NULL; + return PX4_OK; } int logbuffer_count(struct logbuffer_s *lb) @@ -72,6 +72,16 @@ int logbuffer_is_empty(struct logbuffer_s *lb) bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size) { + // allocate buffer if not yet present + if (lb->data == NULL) { + lb->data = malloc(lb->size); + } + + // allocation failed, bail out + if (lb->data == NULL) { + return false; + } + // bytes available to write int available = lb->read_ptr - lb->write_ptr - 1; diff --git a/src/modules/sdlog2/params.c b/src/modules/sdlog2/params.c new file mode 100644 index 000000000000..5878b4aa3dd9 --- /dev/null +++ b/src/modules/sdlog2/params.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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. + * + ****************************************************************************/ + +/** + * Logging rate. + * + * A value of -1 indicates the commandline argument + * should be obeyed. A value of 0 sets the minimum rate, + * any other value is interpreted as rate in Hertz. This + * parameter is only read out before logging starts (which + * commonly is before arming). + * + * @min -1 + * @max 100 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_RATE, -1); + +/** + * Enable extended logging mode. + * + * A value of -1 indicates the commandline argument + * should be obeyed. A value of 0 disables extended + * logging mode, a value of 1 enables it. This + * parameter is only read out before logging starts + * (which commonly is before arming). + * + * @min -1 + * @max 1 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_EXT, -1); + +/** + * Use timestamps only if GPS 3D fix is available + * + * A value of 1 constrains the log folder creation + * to only use the time stamp if a 3D GPS lock is + * present. + * + * @min 0 + * @max 1 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_GPSTIME, 1); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index acc47719689b..f15ec8969ac4 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -44,10 +44,18 @@ #include #include +#include +#include +#include +#include #include #include -#include +#ifdef __PX4_DARWIN +#include +#include +#else #include +#endif #include #include #include @@ -65,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -80,7 +89,7 @@ #include #include #include -#include +#include #include #include #include @@ -105,6 +114,7 @@ #include #include #include +#include #include #include @@ -115,56 +125,13 @@ #define PX4_EPOCH_SECS 1234567890L -/** - * Logging rate. - * - * A value of -1 indicates the commandline argument - * should be obeyed. A value of 0 sets the minimum rate, - * any other value is interpreted as rate in Hertz. This - * parameter is only read out before logging starts (which - * commonly is before arming). - * - * @min -1 - * @max 1 - * @group SD Logging - */ -PARAM_DEFINE_INT32(SDLOG_RATE, -1); - -/** - * Enable extended logging mode. - * - * A value of -1 indicates the commandline argument - * should be obeyed. A value of 0 disables extended - * logging mode, a value of 1 enables it. This - * parameter is only read out before logging starts - * (which commonly is before arming). - * - * @min -1 - * @max 1 - * @group SD Logging - */ -PARAM_DEFINE_INT32(SDLOG_EXT, -1); - -/** - * Use timestamps only if GPS 3D fix is available - * - * A value of 1 constrains the log folder creation - * to only use the time stamp if a 3D GPS lock is - * present. - * - * @min 0 - * @max 1 - * @group SD Logging - */ -PARAM_DEFINE_INT32(SDLOG_GPSTIME, 0); - #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ } else { \ log_msgs_skipped++; \ } -#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) +#define SDLOG_MIN(X,Y) ((X) < (Y) ? (X) : (Y)) static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -179,7 +146,7 @@ static const int MIN_BYTES_TO_WRITE = 512; static bool _extended_logging = false; static bool _gpstime_only = false; -#define MOUNTPOINT "/fs/microsd" +#define MOUNTPOINT PX4_ROOTFSDIR"/fs/microsd" static const char *mountpoint = MOUNTPOINT; static const char *log_root = MOUNTPOINT "/log"; static int mavlink_fd = -1; @@ -189,7 +156,9 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; -static char log_dir[32]; +#define LOG_BASE_PATH_LEN 64 + +static char log_dir[LOG_BASE_PATH_LEN]; /* statistics counters */ static uint64_t start_time = 0; @@ -199,7 +168,8 @@ static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; /* GPS time, used for log files naming */ -static uint64_t gps_time = 0; +static uint64_t gps_time_sec = 0; +static bool has_gps_3d_fix = false; /* current state of logging */ static bool logging_enabled = false; @@ -228,6 +198,7 @@ static void *logwriter_thread(void *arg); __EXPORT int sdlog2_main(int argc, char *argv[]); static bool copy_if_updated(orb_id_t topic, int *handle, void *buffer); +static bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer); /** * Mainloop of sd log deamon. @@ -271,8 +242,6 @@ static int write_parameters(int fd); static bool file_exist(const char *filename); -static int file_copy(const char *file_old, const char *file_new); - /** * Check if there is still free space available */ @@ -345,7 +314,20 @@ int sdlog2_main(int argc, char *argv[]) 3000, sdlog2_thread_main, (char * const *)argv); - return 0; + + /* wait for the task to launch */ + unsigned const max_wait_us = 1000000; + unsigned const max_wait_steps = 2000; + + unsigned i; + for (i = 0; i < max_wait_steps; i++) { + usleep(max_wait_us / max_wait_steps); + if (thread_running) { + break; + } + } + + return !(i < max_wait_steps); } if (!strcmp(argv[1], "stop")) { @@ -393,17 +375,17 @@ int sdlog2_main(int argc, char *argv[]) bool get_log_time_utc_tt(struct tm *tt, bool boot_time) { struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); + px4_clock_gettime(CLOCK_REALTIME, &ts); /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ time_t utc_time_sec; - if (_gpstime_only) { - utc_time_sec = gps_time / 1e6; + if (_gpstime_only && has_gps_3d_fix) { + utc_time_sec = gps_time_sec; } else { utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); } - if (utc_time_sec > PX4_EPOCH_SECS) { + if (utc_time_sec > PX4_EPOCH_SECS) { /* strip the time elapsed since boot */ if (boot_time) { utc_time_sec -= hrt_absolute_time() / 1e6; @@ -443,7 +425,6 @@ int create_log_dir() mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret == 0) { - warnx("log dir created: %s", log_dir); break; } else if (errno != EEXIST) { @@ -464,15 +445,15 @@ int create_log_dir() } /* print logging path, important to find log file later */ - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); + mavlink_and_console_log_info(mavlink_fd, "[log] log dir: %s", log_dir); return 0; } int open_log_file() { /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_name[64] = ""; + char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; struct tm tt; bool time_ok = get_log_time_utc_tt(&tt, false); @@ -500,18 +481,22 @@ int open_log_file() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } - int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC, 0x0777); +#ifdef __PX4_NUTTX + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); +#else + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC, PX4_O_MODE_666); +#endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name); } else { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[log] start: %s", log_file_name); } return fd; @@ -520,8 +505,8 @@ int open_log_file() int open_perf_file(const char* str) { /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_name[64] = ""; + char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; struct tm tt; bool time_ok = get_log_time_utc_tt(&tt, false); @@ -548,15 +533,19 @@ int open_perf_file(const char* str) if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } +#ifdef __PX4_NUTTX + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); +#else int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC, 0x0777); +#endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name); } @@ -683,7 +672,7 @@ void sdlog2_start_log() /* create log dir if needed */ if (create_log_dir() != 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + mavlink_and_console_log_critical(mavlink_fd, "[log] error creating log dir"); return; } @@ -714,9 +703,16 @@ void sdlog2_start_log() } /* write all performance counters */ + hrt_abstime curr_time = hrt_absolute_time(); + struct print_load_s load; int perf_fd = open_perf_file("preflight"); + init_print_load_s(curr_time, &load); + print_load(curr_time, perf_fd, &load); dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n"); perf_print_all(perf_fd); + dprintf(perf_fd, "\nLOAD PRE-FLIGHT\n\n"); + usleep(500 * 1000); + print_load(hrt_absolute_time(), perf_fd, &load); close(perf_fd); /* reset performance counters to get in-flight min and max values in post flight log */ @@ -752,14 +748,21 @@ void sdlog2_stop_log() /* write all performance counters */ int perf_fd = open_perf_file("postflight"); + hrt_abstime curr_time = hrt_absolute_time(); dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n"); perf_print_all(perf_fd); + struct print_load_s load; + dprintf(perf_fd, "\nLOAD POST-FLIGHT\n\n"); + init_print_load_s(curr_time, &load); + print_load(curr_time, perf_fd, &load); + sleep(1); + print_load(hrt_absolute_time(), perf_fd, &load); close(perf_fd); /* free log writer performance counter */ perf_free(perf_write); - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped"); + mavlink_and_console_log_info(mavlink_fd, "[log] logging stopped"); sdlog2_status(); } @@ -843,11 +846,16 @@ int write_parameters(int fd) } bool copy_if_updated(orb_id_t topic, int *handle, void *buffer) +{ + return copy_if_updated_multi(topic, 0, handle, buffer); +} + +bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer) { bool updated = false; if (*handle < 0) { - if (OK == orb_exists(topic, 0)) { + if (OK == orb_exists(topic, multi_instance)) { *handle = orb_subscribe(topic); /* copy first data */ if (*handle >= 0) { @@ -868,7 +876,7 @@ bool copy_if_updated(orb_id_t topic, int *handle, void *buffer) int sdlog2_thread_main(int argc, char *argv[]) { - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { warnx("ERR: log stream, start mavlink app first"); @@ -895,10 +903,12 @@ int sdlog2_thread_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "r:b:eatx")) != EOF) { + int myoptind = 1; + const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "r:b:eatx", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'r': { - unsigned long r = strtoul(optarg, NULL, 10); + unsigned long r = strtoul(myoptarg, NULL, 10); if (r == 0) { r = 1; @@ -909,7 +919,7 @@ int sdlog2_thread_main(int argc, char *argv[]) break; case 'b': { - unsigned long s = strtoul(optarg, NULL, 10); + unsigned long s = strtoul(myoptarg, NULL, 10); if (s < 1) { s = 1; @@ -959,7 +969,7 @@ int sdlog2_thread_main(int argc, char *argv[]) sdlog2_usage(NULL); } - gps_time = 0; + gps_time_sec = 0; /* interpret logging params */ @@ -1030,17 +1040,6 @@ int sdlog2_thread_main(int argc, char *argv[]) return 1; } - /* copy conversion scripts */ - const char *converter_in = "/etc/logging/conv.zip"; - char *converter_out = malloc(64); - snprintf(converter_out, 64, "%s/conv.zip", log_root); - - if (file_copy(converter_in, converter_out) != OK) { - warn("unable to copy conversion scripts"); - } - - free(converter_out); - /* initialize log buffer with specified size */ warnx("log buffer size: %i bytes", log_buffer_size); @@ -1071,7 +1070,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; - struct vehicle_vicon_position_s vicon_pos; + struct att_pos_mocap_s att_pos_mocap; struct vision_position_estimate_s vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; @@ -1092,6 +1091,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vtol_vehicle_status_s vtol_status; struct time_offset_s time_offset; struct mc_att_ctrl_status_s mc_att_ctrl_status; + struct control_state_s ctrl_state; } buf; memset(&buf, 0, sizeof(buf)); @@ -1126,8 +1126,10 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TEL_s log_TEL; struct log_EST0_s log_EST0; struct log_EST1_s log_EST1; + struct log_EST2_s log_EST2; + struct log_EST3_s log_EST3; struct log_PWR_s log_PWR; - struct log_VICN_s log_VICN; + struct log_MOCP_s log_MOCP; struct log_VISN_s log_VISN; struct log_GS0A_s log_GS0A; struct log_GS0B_s log_GS0B; @@ -1138,6 +1140,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ENCD_s log_ENCD; struct log_TSYN_s log_TSYN; struct log_MACS_s log_MACS; + struct log_CTS_s log_CTS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1162,7 +1165,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int triplet_sub; int gps_pos_sub; int sat_info_sub; - int vicon_pos_sub; + int att_pos_mocap_sub; int vision_pos_sub; int flow_sub; int rc_sub; @@ -1170,7 +1173,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; - int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; + int telemetry_subs[ORB_MULTI_MAX_INSTANCES]; int distance_sensor_sub; int estimator_status_sub; int tecs_status_sub; @@ -1180,6 +1183,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int encoders_sub; int tsync_sub; int mc_att_ctrl_status_sub; + int ctrl_state_sub; } subs; subs.cmd_sub = -1; @@ -1197,7 +1201,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.local_pos_sp_sub = -1; subs.global_pos_sub = -1; subs.triplet_sub = -1; - subs.vicon_pos_sub = -1; + subs.att_pos_mocap_sub = -1; subs.vision_pos_sub = -1; subs.flow_sub = -1; subs.rc_sub = -1; @@ -1213,43 +1217,38 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.wind_sub = -1; subs.tsync_sub = -1; subs.mc_att_ctrl_status_sub = -1; + subs.ctrl_state_sub = -1; subs.encoders_sub = -1; /* add new topics HERE */ - for (unsigned i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { subs.telemetry_subs[i] = -1; } subs.sat_info_sub = -1; - /* close non-needed fd's */ +#ifdef __PX4_NUTTX + /* close non-needed fd's. We cannot do this for posix since the file + descriptors will also be closed for the parent process + */ /* close stdin */ close(0); /* close stdout */ close(1); - - thread_running = true; - +#endif /* initialize thread synchronization */ pthread_mutex_init(&logbuffer_mutex, NULL); pthread_cond_init(&logbuffer_cond, NULL); /* track changes in sensor_combined topic */ - hrt_abstime gyro_timestamp = 0; - hrt_abstime accelerometer_timestamp = 0; - hrt_abstime magnetometer_timestamp = 0; - hrt_abstime barometer_timestamp = 0; - hrt_abstime differential_pressure_timestamp = 0; - hrt_abstime barometer1_timestamp = 0; - hrt_abstime gyro1_timestamp = 0; - hrt_abstime accelerometer1_timestamp = 0; - hrt_abstime magnetometer1_timestamp = 0; - hrt_abstime gyro2_timestamp = 0; - hrt_abstime accelerometer2_timestamp = 0; - hrt_abstime magnetometer2_timestamp = 0; + hrt_abstime gyro_timestamp[3] = {0, 0, 0}; + hrt_abstime accelerometer_timestamp[3] = {0, 0, 0}; + hrt_abstime magnetometer_timestamp[3] = {0, 0, 0}; + hrt_abstime barometer_timestamp[3] = {0, 0, 0}; + hrt_abstime differential_pressure_timestamp[3] = {0, 0, 0}; /* initialize calculated mean SNR */ float snr_mean = 0.0f; @@ -1259,13 +1258,16 @@ int sdlog2_thread_main(int argc, char *argv[]) /* check GPS topic to get GPS time */ if (log_name_timestamp) { if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { - gps_time = buf_gps_pos.time_utc_usec / 1e6; + gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; } } sdlog2_start_log(); } + /* running, report */ + thread_running = true; + while (!main_thread_should_exit) { usleep(sleep_delay); @@ -1287,7 +1289,8 @@ int sdlog2_thread_main(int argc, char *argv[]) bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { - gps_time = buf_gps_pos.time_utc_usec / 1e6; + gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; + has_gps_3d_fix = buf_gps_pos.fix_type == 3; } if (!logging_enabled) { @@ -1349,7 +1352,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (copy_if_updated(ORB_ID(satellite_info), &subs.sat_info_sub, &buf.sat_info)) { /* log the SNR of each satellite for a detailed view of signal quality */ - unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0])); + unsigned sat_info_count = SDLOG_MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0])); unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]); log_msg.msg_type = LOG_GS0A_MSG; @@ -1393,144 +1396,86 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SENSOR COMBINED --- */ if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) { - bool write_IMU = false; - bool write_IMU1 = false; - bool write_IMU2 = false; - bool write_SENS = false; - bool write_SENS1 = false; - - if (buf.sensor.timestamp != gyro_timestamp) { - gyro_timestamp = buf.sensor.timestamp; - write_IMU = true; - } - - if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) { - accelerometer_timestamp = buf.sensor.accelerometer_timestamp; - write_IMU = true; - } - - if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) { - magnetometer_timestamp = buf.sensor.magnetometer_timestamp; - write_IMU = true; - } - - if (buf.sensor.baro_timestamp != barometer_timestamp) { - barometer_timestamp = buf.sensor.baro_timestamp; - write_SENS = true; - } - - if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) { - differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp; - write_SENS = true; - } - - if (write_IMU) { - log_msg.msg_type = LOG_IMU_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } + - if (write_SENS) { - log_msg.msg_type = LOG_SENS_MSG; - log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; - log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; - log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; - log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; - log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa; - LOGBUFFER_WRITE_AND_COUNT(SENS); - } - - if (buf.sensor.baro1_timestamp != barometer1_timestamp) { - barometer1_timestamp = buf.sensor.baro1_timestamp; - write_SENS1 = true; - } + for (unsigned i = 0; i < 3; i++) { + bool write_IMU = false; + bool write_SENS = false; - if (write_SENS1) { - log_msg.msg_type = LOG_AIR1_MSG; - log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar; - log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter; - log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius; - log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa; - log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa; - // XXX moving to AIR0-AIR2 instead of SENS - LOGBUFFER_WRITE_AND_COUNT(SENS); - } + if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) { + gyro_timestamp[i] = buf.sensor.gyro_timestamp[i]; + write_IMU = true; + } - if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) { - accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp; - write_IMU1 = true; - } + if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) { + accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i]; + write_IMU = true; + } - if (buf.sensor.gyro1_timestamp != gyro1_timestamp) { - gyro1_timestamp = buf.sensor.gyro1_timestamp; - write_IMU1 = true; - } + if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) { + magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i]; + write_IMU = true; + } - if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) { - magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp; - write_IMU1 = true; - } + if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) { + barometer_timestamp[i] = buf.sensor.baro_timestamp[i]; + write_SENS = true; + } - if (write_IMU1) { - log_msg.msg_type = LOG_IMU1_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro1_temp; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer1_temp; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer1_temp; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } + if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) { + differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i]; + write_SENS = true; + } - if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) { - accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp; - write_IMU2 = true; - } + if (write_IMU) { + switch (i) { + case 0: + log_msg.msg_type = LOG_IMU_MSG; + break; + case 1: + log_msg.msg_type = LOG_IMU1_MSG; + break; + case 2: + log_msg.msg_type = LOG_IMU2_MSG; + break; + } - if (buf.sensor.gyro2_timestamp != gyro2_timestamp) { - gyro2_timestamp = buf.sensor.gyro2_timestamp; - write_IMU2 = true; - } + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i * 3 + 0]; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i * 3 + 0]; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i * 3 + 0]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } - if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) { - magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp; - write_IMU2 = true; - } + if (write_SENS) { + switch (i) { + case 0: + log_msg.msg_type = LOG_SENS_MSG; + break; + case 1: + log_msg.msg_type = LOG_AIR1_MSG; + break; + case 2: + continue; + break; + } - if (write_IMU2) { - log_msg.msg_type = LOG_IMU2_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro2_temp; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer2_temp; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer2_temp; - LOGBUFFER_WRITE_AND_COUNT(IMU); + log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i]; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i]; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i]; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i]; + log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i]; + LOGBUFFER_WRITE_AND_COUNT(SENS); + } } - } /* --- ATTITUDE --- */ @@ -1681,16 +1626,17 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- VICON POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_vicon_position), &subs.vicon_pos_sub, &buf.vicon_pos)) { - log_msg.msg_type = LOG_VICN_MSG; - log_msg.body.log_VICN.x = buf.vicon_pos.x; - log_msg.body.log_VICN.y = buf.vicon_pos.y; - log_msg.body.log_VICN.z = buf.vicon_pos.z; - log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch; - log_msg.body.log_VICN.roll = buf.vicon_pos.roll; - log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; - LOGBUFFER_WRITE_AND_COUNT(VICN); + /* --- MOCAP ATTITUDE AND POSITION --- */ + if (copy_if_updated(ORB_ID(att_pos_mocap), &subs.att_pos_mocap_sub, &buf.att_pos_mocap)) { + log_msg.msg_type = LOG_MOCP_MSG; + log_msg.body.log_MOCP.qw = buf.att_pos_mocap.q[0]; + log_msg.body.log_MOCP.qx = buf.att_pos_mocap.q[1]; + log_msg.body.log_MOCP.qy = buf.att_pos_mocap.q[2]; + log_msg.body.log_MOCP.qz = buf.att_pos_mocap.q[3]; + log_msg.body.log_MOCP.x = buf.att_pos_mocap.x; + log_msg.body.log_MOCP.y = buf.att_pos_mocap.y; + log_msg.body.log_MOCP.z = buf.att_pos_mocap.z; + LOGBUFFER_WRITE_AND_COUNT(MOCP); } /* --- VISION POSITION --- */ @@ -1730,6 +1676,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.rssi = buf.rc.rssi; log_msg.body.log_RC.channel_count = buf.rc.channel_count; log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; LOGBUFFER_WRITE_AND_COUNT(RC); @@ -1802,8 +1749,8 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TELEMETRY --- */ - for (unsigned i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (copy_if_updated(telemetry_status_orb_id[i], &subs.telemetry_subs[i], &buf.telemetry)) { + for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (copy_if_updated_multi(ORB_ID(telemetry_status), i, &subs.telemetry_subs[i], &buf.telemetry)) { log_msg.msg_type = LOG_TEL0_MSG + i; log_msg.body.log_TEL.rssi = buf.telemetry.rssi; log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi; @@ -1845,6 +1792,18 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s)); memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1); LOGBUFFER_WRITE_AND_COUNT(EST1); + + log_msg.msg_type = LOG_EST2_MSG; + unsigned maxcopy2 = (sizeof(buf.estimator_status.covariances) < sizeof(log_msg.body.log_EST2.cov)) ? sizeof(buf.estimator_status.covariances) : sizeof(log_msg.body.log_EST2.cov); + memset(&(log_msg.body.log_EST2.cov), 0, sizeof(log_msg.body.log_EST2.cov)); + memcpy(&(log_msg.body.log_EST2.cov), buf.estimator_status.covariances, maxcopy2); + LOGBUFFER_WRITE_AND_COUNT(EST2); + + log_msg.msg_type = LOG_EST3_MSG; + unsigned maxcopy3 = ((sizeof(buf.estimator_status.covariances) - maxcopy2) < sizeof(log_msg.body.log_EST3.cov)) ? (sizeof(buf.estimator_status.covariances) - maxcopy2) : sizeof(log_msg.body.log_EST3.cov); + memset(&(log_msg.body.log_EST3.cov), 0, sizeof(log_msg.body.log_EST3.cov)); + memcpy(&(log_msg.body.log_EST3.cov), buf.estimator_status.covariances + maxcopy2, maxcopy3); + LOGBUFFER_WRITE_AND_COUNT(EST3); } /* --- TECS STATUS --- */ @@ -1854,15 +1813,16 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; - log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; - log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; - log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; - log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; - log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError; + log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError; + log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError; + log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError; + log_msg.body.log_TECS.pitch_integ = buf.tecs_status.pitch_integ; + log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ; log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; LOGBUFFER_WRITE_AND_COUNT(TECS); } @@ -1903,6 +1863,19 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(MACS); } + /* --- CONTROL STATE --- */ + if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) { + log_msg.msg_type = LOG_CTS_MSG; + log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel; + log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel; + log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel; + log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed; + log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate; + log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate; + log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate; + LOGBUFFER_WRITE_AND_COUNT(CTS); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ @@ -1930,7 +1903,7 @@ int sdlog2_thread_main(int argc, char *argv[]) void sdlog2_status() { warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); - warnx("time: gps: %u seconds", (unsigned)gps_time); + warnx("time: gps: %u seconds", (unsigned)gps_time_sec); if (!logging_enabled) { warnx("not logging"); } else { @@ -1940,12 +1913,12 @@ void sdlog2_status() float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[log] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } } /** - * @return 0 if file exists + * @return true if file exists */ bool file_exist(const char *filename) { @@ -1953,46 +1926,6 @@ bool file_exist(const char *filename) return stat(filename, &buffer) == 0; } -int file_copy(const char *file_old, const char *file_new) -{ - FILE *source, *target; - source = fopen(file_old, "r"); - int ret = 0; - - if (source == NULL) { - warnx("ERR: open in"); - return PX4_ERROR; - } - - target = fopen(file_new, "w"); - - if (target == NULL) { - fclose(source); - warnx("ERR: open out"); - return PX4_ERROR; - } - - char buf[128]; - int nread; - - while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) { - ret = fwrite(buf, 1, nread, target); - - if (ret <= 0) { - warnx("error writing file"); - ret = PX4_ERROR; - break; - } - } - - fsync(fileno(target)); - - fclose(source); - fclose(target); - - return PX4_OK; -} - int check_free_space() { /* use statfs to determine the number of blocks left */ @@ -2005,7 +1938,7 @@ int check_free_space() /* use a threshold of 50 MiB */ if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, - "[sdlog2] no space on MicroSD: %u MiB", + "[log] no space on MicroSD: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return PX4_ERROR; @@ -2013,7 +1946,7 @@ int check_free_space() /* use a threshold of 100 MiB to send a warning */ } else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, - "[sdlog2] space on MicroSD low: %u MiB", + "[log] space on MicroSD low: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we don't want to flood the user with warnings */ space_warning_sent = true; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9cf37683aea8..6ccbec2fa938 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -165,7 +165,7 @@ struct log_GPS_s { /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */ #define LOG_ATTC_MSG 9 -#define LOG_ATC1_MSG 40 +#define LOG_ATC1_MSG 46 struct log_ATTC_s { float roll; float pitch; @@ -189,6 +189,7 @@ struct log_STAT_s { #define LOG_RC_MSG 11 struct log_RC_s { float channel[8]; + uint8_t rssi; uint8_t channel_count; uint8_t signal_lost; }; @@ -218,7 +219,6 @@ struct log_ARSP_s { /* --- FLOW - OPTICAL FLOW --- */ #define LOG_FLOW_MSG 15 struct log_FLOW_s { - uint64_t timestamp; uint8_t sensor_id; float pixel_flow_x_integral; float pixel_flow_y_integral; @@ -321,15 +321,16 @@ struct log_PWR_s { uint8_t high_power_rail_overcurrent; }; -/* --- VICN - VICON POSITION --- */ -#define LOG_VICN_MSG 25 -struct log_VICN_s { +/* --- MOCP - MOCAP ATTITUDE AND POSITION --- */ +#define LOG_MOCP_MSG 25 +struct log_MOCP_s { + float qw; + float qx; + float qy; + float qz; float x; float y; float z; - float roll; - float pitch; - float yaw; }; /* --- GS0A - GPS SNR #0, SAT GROUP A --- */ @@ -363,16 +364,16 @@ struct log_TECS_s { float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; - float flightPathAngleFiltered; float airspeedSp; float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; - - float totalEnergyRateSp; - float totalEnergyRate; - float energyDistributionRateSp; - float energyDistributionRate; + float totalEnergyError; + float totalEnergyRateError; + float energyDistributionError; + float energyDistributionRateError; + float pitch_integ; + float throttle_integ; uint8_t mode; }; @@ -402,11 +403,23 @@ struct log_EST1_s { float s[16]; }; +/* --- EST2 - ESTIMATOR STATUS --- */ +#define LOG_EST2_MSG 34 +struct log_EST2_s { + float cov[12]; +}; + +/* --- EST3 - ESTIMATOR STATUS --- */ +#define LOG_EST3_MSG 35 +struct log_EST3_s { + float cov[16]; +}; + /* --- TEL0..3 - TELEMETRY STATUS --- */ -#define LOG_TEL0_MSG 34 -#define LOG_TEL1_MSG 35 -#define LOG_TEL2_MSG 36 -#define LOG_TEL3_MSG 37 +#define LOG_TEL0_MSG 36 +#define LOG_TEL1_MSG 37 +#define LOG_TEL2_MSG 38 +#define LOG_TEL3_MSG 39 struct log_TEL_s { uint8_t rssi; uint8_t remote_rssi; @@ -419,7 +432,7 @@ struct log_TEL_s { }; /* --- VISN - VISION POSITION --- */ -#define LOG_VISN_MSG 38 +#define LOG_VISN_MSG 40 struct log_VISN_s { float x; float y; @@ -427,14 +440,14 @@ struct log_VISN_s { float vx; float vy; float vz; + float qw; float qx; float qy; float qz; - float qw; }; /* --- ENCODERS - ENCODER DATA --- */ -#define LOG_ENCD_MSG 39 +#define LOG_ENCD_MSG 41 struct log_ENCD_s { int64_t cnt0; float vel0; @@ -443,28 +456,42 @@ struct log_ENCD_s { }; /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ -#define LOG_AIR1_MSG 41 +#define LOG_AIR1_MSG 42 /* --- VTOL - VTOL VEHICLE STATUS */ -#define LOG_VTOL_MSG 42 +#define LOG_VTOL_MSG 43 struct log_VTOL_s { float airspeed_tot; }; /* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ -#define LOG_TSYN_MSG 43 +#define LOG_TSYN_MSG 44 struct log_TSYN_s { uint64_t time_offset; }; /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ -#define LOG_MACS_MSG 44 +#define LOG_MACS_MSG 45 struct log_MACS_s { float roll_rate_integ; float pitch_rate_integ; float yaw_rate_integ; }; +/* --- CONTROL STATE --- */ +#define LOG_CTS_MSG 47 +struct log_CTS_s { + float vx_body; + float vy_body; + float vz_body; + float airspeed; + float roll_rate; + float pitch_rate; + float yaw_rate; +}; + +/* WARNING: ID 46 is already in use for ATTC1 */ + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -505,11 +532,12 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), - LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), + LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), + LOG_FORMAT(RC, "ffffffffBBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,RSSI,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), - LOG_FORMAT(FLOW, "QBffffffLLHhB", "IntT,ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"), + LOG_FORMAT(FLOW, "BffffffLLHhB", "ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), @@ -522,14 +550,16 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), + LOG_FORMAT(EST2, "ffffffffffff", "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11"), + LOG_FORMAT(EST3, "ffffffffffffffff", "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), - LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), - LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"), + LOG_FORMAT(MOCP, "fffffff", "QuatW,QuatX,QuatY,QuatZ,X,Y,Z"), + LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatW,QuatX,QuatY,QuatZ"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,ERE,EDE,EDRE,PtchI,ThrI,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(TSYN, "Q", "TimeOffset"), diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 91230a37cb9b..ba7665201a2a 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -1,8 +1,9 @@ #include "BlockSegwayController.hpp" -void BlockSegwayController::update() { +void BlockSegwayController::update() +{ // wait for a sensor update, check for exit condition every 100 ms - if (poll(&_attPoll, 1, 100) < 0) return; // poll error + if (poll(&_attPoll, 1, 100) < 0) { return; } // poll error uint64_t newTimeStamp = hrt_absolute_time(); float dt = (newTimeStamp - _timeStamp) / 1.0e6f; @@ -10,20 +11,21 @@ void BlockSegwayController::update() { // check for sane values of dt // to prevent large control responses - if (dt > 1.0f || dt < 0) return; + if (dt > 1.0f || dt < 0) { return; } // set dt for all child blocks setDt(dt); // check for new updates - if (_param_update.updated()) updateParams(); + if (_param_update.updated()) { updateParams(); } // get new information from subscriptions updateSubscriptions(); // default all output to zero unless handled by mode - for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) + for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) { _actuators.control[i] = 0.0f; + } // only update guidance in auto mode if (_status.main_state == MAIN_STATE_AUTO) { diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp index 4a01f785c52c..4413a3cff861 100644 --- a/src/modules/segway/BlockSegwayController.hpp +++ b/src/modules/segway/BlockSegwayController.hpp @@ -4,10 +4,11 @@ using namespace control; -class BlockSegwayController : public control::BlockUorbEnabledAutopilot { +class BlockSegwayController : public control::BlockUorbEnabledAutopilot +{ public: BlockSegwayController() : - BlockUorbEnabledAutopilot(NULL,"SEG"), + BlockUorbEnabledAutopilot(NULL, "SEG"), th2v(this, "TH2V"), q2v(this, "Q2V"), _attPoll(), diff --git a/src/modules/segway/CMakeLists.txt b/src/modules/segway/CMakeLists.txt new file mode 100644 index 000000000000..6e9dd93bb268 --- /dev/null +++ b/src/modules/segway/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__segway + MAIN segway + SRCS + segway_main.cpp + BlockSegwayController.cpp + params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index 67acd94bf701..667b2ad3fe98 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -74,8 +74,9 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: segway {start|stop|status} [-p ]\n\n"); exit(1); @@ -107,11 +108,11 @@ int segway_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd("segway", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 5120, - segway_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 5120, + segway_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt new file mode 100644 index 000000000000..e296ebe103c4 --- /dev/null +++ b/src/modules/sensors/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__sensors + MAIN sensors + PRIORITY "SCHED_PRIORITY_MAX-5" + STACK 1200 + COMPILE_FLAGS + -Wno-type-limits + -O3 + SRCS + sensors.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c8ad86afa7b9..3d4dfd62c185 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -585,6 +585,34 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f); +/** + * Primary accel ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0); + +/** + * Primary gyro ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0); + +/** + * Primary mag ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0); + +/** + * Primary baro ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0); + /** * Differential pressure sensor offset * @@ -680,6 +708,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user * to fine tune the board offset in the event of misalignment. * + * @unit degrees * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); @@ -690,80 +719,1014 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user * to fine tune the board offset in the event of misalignment. * + * @unit degrees * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); /** - * Board rotation Z (YAW) offset + * Board rotation Z (YAW) offset + * + * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @unit degrees + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); + +/** + * External magnetometer rotation + * + * This parameter defines the rotation of the external magnetometer relative + * to the platform (not relative to the FMU). + * See SENS_BOARD_ROT for possible values. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); + +/** +* Set usage of external magnetometer +* +* * Set to 0 (default) to auto-detect (will try to get the external as primary) +* * Set to 1 to force the external magnetometer as primary +* * Set to 2 to force the internal magnetometer as primary +* +* @min 0 +* @max 2 +* @group Sensor Calibration +*/ +PARAM_DEFINE_INT32(SENS_EXT_MAG, 0); + + +/** + * RC Channel 1 Minimum + * + * Minimum value for RC channel 1 + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); + +/** + * RC Channel 1 Trim + * + * Mid point value (same as min for throttle) + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); + +/** + * RC Channel 1 Maximum + * + * Maximum value for RC channel 1 + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); + +/** + * RC Channel 1 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); + +/** + * RC Channel 1 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); + +/** + * RC Channel 2 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); + +/** + * RC Channel 2 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f); + +/** + * RC Channel 2 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); + +/** + * RC Channel 2 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); + +/** + * RC Channel 2 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); + +/** + * RC Channel 3 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_MIN, 1000); + +/** + * RC Channel 3 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); + +/** + * RC Channel 3 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_MAX, 2000); + +/** + * RC Channel 3 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); +/** + * RC Channel 3 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); + +/** + * RC Channel 4 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_MIN, 1000); + +/** + * RC Channel 4 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); + +/** + * RC Channel 4 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_MAX, 2000); + +/** + * RC Channel 4 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); + +/** + * RC Channel 4 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f); + +/** + * RC Channel 5 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_MIN, 1000); + +/** + * RC Channel 5 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); + +/** + * RC Channel 5 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_MAX, 2000); + +/** + * RC Channel 5 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); + +/** + * RC Channel 5 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f); + +/** + * RC Channel 6 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_MIN, 1000); + +/** + * RC Channel 6 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); + +/** + * RC Channel 6 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_MAX, 2000); + +/** + * RC Channel 6 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); + +/** + * RC Channel 6 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f); + +/** + * RC Channel 7 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_MIN, 1000); + +/** + * RC Channel 7 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); + +/** + * RC Channel 7 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_MAX, 2000); + +/** + * RC Channel 7 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); + +/** + * RC Channel 7 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f); + +/** + * RC Channel 8 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_MIN, 1000); + +/** + * RC Channel 8 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); + +/** + * RC Channel 8 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_MAX, 2000); + +/** + * RC Channel 8 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); + +/** + * RC Channel 8 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f); + +/** + * RC Channel 9 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_MIN, 1000); + +/** + * RC Channel 9 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); + +/** + * RC Channel 9 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_MAX, 2000); + +/** + * RC Channel 9 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_REV, 1.0f); + +/** + * RC Channel 9 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f); + +/** + * RC Channel 10 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_MIN, 1000); + +/** + * RC Channel 10 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_TRIM, 1500); + +/** + * RC Channel 10 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_MAX, 2000); + +/** + * RC Channel 10 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_REV, 1.0f); + +/** + * RC Channel 10 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f); + +/** + * RC Channel 11 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_MIN, 1000); + +/** + * RC Channel 11 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_TRIM, 1500); + +/** + * RC Channel 11 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_MAX, 2000); + +/** + * RC Channel 11 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_REV, 1.0f); + +/** + * RC Channel 11 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f); + +/** + * RC Channel 12 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_MIN, 1000); + +/** + * RC Channel 12 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_TRIM, 1500); + +/** + * RC Channel 12 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_MAX, 2000); + +/** + * RC Channel 12 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_REV, 1.0f); + +/** + * RC Channel 12 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f); + +/** + * RC Channel 13 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_MIN, 1000); + +/** + * RC Channel 13 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_TRIM, 1500); + +/** + * RC Channel 13 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_MAX, 2000); + +/** + * RC Channel 13 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_REV, 1.0f); + +/** + * RC Channel 13 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f); + +/** + * RC Channel 14 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC14_MIN, 1000); + +/** + * RC Channel 14 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC14_TRIM, 1500); + +/** + * RC Channel 14 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC14_MAX, 2000); + +/** + * RC Channel 14 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); + +/** + * RC Channel 14 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); + +/** + * RC Channel 15 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC15_MIN, 1000); + +/** + * RC Channel 15 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); + +/** + * RC Channel 15 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC15_MAX, 2000); + +/** + * RC Channel 15 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); + +/** + * RC Channel 15 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); + +/** + * RC Channel 16 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC16_MIN, 1000); + +/** + * RC Channel 16 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC16_TRIM, 1500); + +/** + * RC Channel 16 Maximum * - * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user - * to fine tune the board offset in the event of misalignment. + * Maximum value for this channel. * - * @group Sensor Calibration + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); +PARAM_DEFINE_FLOAT(RC16_MAX, 2000); /** - * External magnetometer rotation + * RC Channel 16 Reverse * - * This parameter defines the rotation of the external magnetometer relative - * to the platform (not relative to the FMU). - * See SENS_BOARD_ROT for possible values. + * Set to -1 to reverse channel. * - * @group Sensor Calibration + * @min -1.0 + * @max 1.0 + * @group Radio Calibration */ -PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); +PARAM_DEFINE_FLOAT(RC16_REV, 1.0f); /** -* Set usage of external magnetometer -* -* * Set to 0 (default) to auto-detect (will try to get the external as primary) -* * Set to 1 to force the external magnetometer as primary -* * Set to 2 to force the internal magnetometer as primary -* -* @min 0 -* @max 2 -* @group Sensor Calibration -*/ -PARAM_DEFINE_INT32(SENS_EXT_MAG, 0); - + * RC Channel 16 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f); /** - * RC Channel 1 Minimum + * RC Channel 17 Minimum * - * Minimum value for RC channel 1 + * Minimum value for this channel. * * @min 800.0 * @max 1500.0 + * @unit us * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); +PARAM_DEFINE_FLOAT(RC17_MIN, 1000); /** - * RC Channel 1 Trim + * RC Channel 17 Trim * - * Mid point value (same as min for throttle) + * Mid point value (has to be set to the same as min for throttle channel). * * @min 800.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); +PARAM_DEFINE_FLOAT(RC17_TRIM, 1500); /** - * RC Channel 1 Maximum + * RC Channel 17 Maximum * - * Maximum value for RC channel 1 + * Maximum value for this channel. * * @min 1500.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); +PARAM_DEFINE_FLOAT(RC17_MAX, 2000); /** - * RC Channel 1 Reverse + * RC Channel 17 Reverse * * Set to -1 to reverse channel. * @@ -771,10 +1734,10 @@ PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); * @max 1.0 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC17_REV, 1.0f); /** - * RC Channel 1 dead zone + * RC Channel 17 dead zone * * The +- range of this value around the trim value will be considered as zero. * @@ -782,43 +1745,46 @@ PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); * @max 100.0 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); +PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f); /** - * RC Channel 2 Minimum + * RC Channel 18 Minimum * - * Minimum value for RC channel 2 + * Minimum value for this channel. * * @min 800.0 * @max 1500.0 + * @unit us * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); +PARAM_DEFINE_FLOAT(RC18_MIN, 1000); /** - * RC Channel 2 Trim + * RC Channel 18 Trim * - * Mid point value (same as min for throttle) + * Mid point value (has to be set to the same as min for throttle channel). * * @min 800.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f); +PARAM_DEFINE_FLOAT(RC18_TRIM, 1500); /** - * RC Channel 2 Maximum + * RC Channel 18 Maximum * - * Maximum value for RC channel 2 + * Maximum value for this channel. * * @min 1500.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); +PARAM_DEFINE_FLOAT(RC18_MAX, 2000); /** - * RC Channel 2 Reverse + * RC Channel 18 Reverse * * Set to -1 to reverse channel. * @@ -826,10 +1792,10 @@ PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); * @max 1.0 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC18_REV, 1.0f); /** - * RC Channel 2 dead zone + * RC Channel 18 dead zone * * The +- range of this value around the trim value will be considered as zero. * @@ -837,105 +1803,16 @@ PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); * @max 100.0 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC3_MIN, 1000); -PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC3_MAX, 2000); -PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC4_MIN, 1000); -PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC4_MAX, 2000); -PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC5_MIN, 1000); -PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC5_MAX, 2000); -PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC6_MIN, 1000); -PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC6_MAX, 2000); -PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC7_MIN, 1000); -PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC7_MAX, 2000); -PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC8_MIN, 1000); -PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC8_MAX, 2000); -PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f); - -PARAM_DEFINE_FLOAT(RC9_MIN, 1000); -PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC9_MAX, 2000); -PARAM_DEFINE_FLOAT(RC9_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC10_MIN, 1000); -PARAM_DEFINE_FLOAT(RC10_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC10_MAX, 2000); -PARAM_DEFINE_FLOAT(RC10_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC11_MIN, 1000); -PARAM_DEFINE_FLOAT(RC11_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC11_MAX, 2000); -PARAM_DEFINE_FLOAT(RC11_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC12_MIN, 1000); -PARAM_DEFINE_FLOAT(RC12_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC12_MAX, 2000); -PARAM_DEFINE_FLOAT(RC12_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC13_MIN, 1000); -PARAM_DEFINE_FLOAT(RC13_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC13_MAX, 2000); -PARAM_DEFINE_FLOAT(RC13_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC14_MIN, 1000); -PARAM_DEFINE_FLOAT(RC14_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC14_MAX, 2000); -PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC15_MIN, 1000); -PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC15_MAX, 2000); -PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC16_MIN, 1000); -PARAM_DEFINE_FLOAT(RC16_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC16_MAX, 2000); -PARAM_DEFINE_FLOAT(RC16_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC17_MIN, 1000); -PARAM_DEFINE_FLOAT(RC17_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC17_MAX, 2000); -PARAM_DEFINE_FLOAT(RC17_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f); - -PARAM_DEFINE_FLOAT(RC18_MIN, 1000); -PARAM_DEFINE_FLOAT(RC18_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC18_MAX, 2000); -PARAM_DEFINE_FLOAT(RC18_REV, 1.0f); PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +/** + * Enable relay control of relay 1 mapped to the Spektrum receiver power supply + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif @@ -952,40 +1829,18 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /** * Scaling factor for battery voltage sensor on PX4IO. * + * @min 1 + * @max 100000 * @group Battery Calibration */ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 /** * Scaling factor for battery voltage sensor on FMU v2. * - * @board CONFIG_ARCH_BOARD_PX4FMU_V2 - * @group Battery Calibration - */ -PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); -#elif CONFIG_ARCH_BOARD_AEROCORE -/** - * Scaling factor for battery voltage sensor on AeroCore. - * - * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063 - * - * @board CONFIG_ARCH_BOARD_AEROCORE - * @group Battery Calibration - */ -PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f); -#else -/** - * Scaling factor for battery voltage sensor on FMU v1. - * - * FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 - * FMUv1 with PX4IO: 0.00459340659 - * FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238 - * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); -#endif +PARAM_DEFINE_FLOAT(BAT_V_SCALING, -1.0f); /** * Scaling factor for battery current sensor. @@ -1036,7 +1891,7 @@ PARAM_DEFINE_INT32(RC_TH_USER, 1); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); +PARAM_DEFINE_INT32(RC_MAP_ROLL, 0); /** * Pitch control channel mapping. @@ -1049,7 +1904,7 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); +PARAM_DEFINE_INT32(RC_MAP_PITCH, 0); /** * Failsafe channel mapping. @@ -1076,7 +1931,7 @@ PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); +PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 0); /** * Yaw control channel mapping. @@ -1089,7 +1944,7 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_YAW, 4); +PARAM_DEFINE_INT32(RC_MAP_YAW, 0); /** * Mode switch channel mapping. @@ -1114,6 +1969,15 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); +/** + * Rattitude switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Switches + */ +PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0); + /** * Posctl switch channel mapping. * @@ -1231,8 +2095,12 @@ PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0); /** * Failsafe channel PWM threshold. * - * @min 800 + * Set to a value slightly above the PWM value assumed by throttle in a failsafe event, + * but ensure it is below the PWM value assumed by throttle during normal operation. + * + * @min 0 * @max 2200 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); @@ -1274,7 +2142,7 @@ PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting posctl mode + * Threshold for selecting rattitude mode * * 0-1 indicate where in the full channel range the threshold sits * 0 : min @@ -1285,7 +2153,24 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * * @min -1 * @max 1 + * @group Radio Switches + * + */ +PARAM_DEFINE_FLOAT(RC_RATT_TH, 0.5f); + +/** + * Threshold for selecting posctl mode + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel */ +#include + #include #include #include @@ -111,26 +113,10 @@ * IO: * IN4 - servo supply rail * IN5 - analog RSSI + * + * The channel definitions (e.g., ADC_BATTERY_VOLTAGE_CHANNEL, ADC_BATTERY_CURRENT_CHANNEL, and ADC_AIRSPEED_VOLTAGE_CHANNEL) are defined in board_config.h */ -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 -#define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 -#endif - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 -#define ADC_BATTERY_VOLTAGE_CHANNEL 2 -#define ADC_BATTERY_CURRENT_CHANNEL 3 -#define ADC_5V_RAIL_SENSE 4 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 -#endif - -#ifdef CONFIG_ARCH_BOARD_AEROCORE -#define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) -#define ADC_AIRSPEED_VOLTAGE_CHANNEL ((uint8_t)(-1)) -#endif #define BATT_V_LOWPASS 0.001f #define BATT_V_IGNORE_THRESHOLD 2.5f @@ -143,6 +129,8 @@ #define STICK_ON_OFF_LIMIT 0.75f #define MAG_ROT_VAL_INTERNAL -1 +#define SENSOR_COUNT_MAX 3 + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -179,7 +167,8 @@ class Sensors int start(); private: - static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = + input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ /** * Get and limit value for specified RC function. Returns NAN if not mapped. @@ -214,25 +203,23 @@ class Sensors bool _hil_enabled; /**< if true, HIL is active */ bool _publishing; /**< if true, we are publishing sensor data */ + bool _armed; /**< arming status of the vehicle */ + + int _gyro_sub[SENSOR_COUNT_MAX]; /**< raw gyro data subscription */ + int _accel_sub[SENSOR_COUNT_MAX]; /**< raw accel data subscription */ + int _mag_sub[SENSOR_COUNT_MAX]; /**< raw mag data subscription */ + int _baro_sub[SENSOR_COUNT_MAX]; /**< raw baro data subscription */ + unsigned _gyro_count; /**< raw gyro data count */ + unsigned _accel_count; /**< raw accel data count */ + unsigned _mag_count; /**< raw mag data count */ + unsigned _baro_count; /**< raw baro data count */ - int _gyro_sub; /**< raw gyro0 data subscription */ - int _accel_sub; /**< raw accel0 data subscription */ - int _mag_sub; /**< raw mag0 data subscription */ - int _gyro1_sub; /**< raw gyro1 data subscription */ - int _accel1_sub; /**< raw accel1 data subscription */ - int _mag1_sub; /**< raw mag1 data subscription */ - int _gyro2_sub; /**< raw gyro2 data subscription */ - int _accel2_sub; /**< raw accel2 data subscription */ - int _mag2_sub; /**< raw mag2 data subscription */ int _rc_sub; /**< raw rc channels data subscription */ - int _baro_sub; /**< raw baro data subscription */ - int _baro1_sub; /**< raw baro data subscription */ - //int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ - int _vcontrol_mode_sub; /**< vehicle control mode subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ - int _rc_parameter_map_sub; /**< rc parameter map subscription */ - int _manual_control_sub; /**< notification of manual control updates */ + int _rc_parameter_map_sub; /**< rc parameter map subscription */ + int _manual_control_sub; /**< notification of manual control updates */ orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ @@ -250,10 +237,10 @@ class Sensors struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; struct rc_parameter_map_s _rc_parameter_map; - float _param_rc_values[RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */ + float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */ math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ + math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -282,6 +269,7 @@ class Sensors int rc_map_mode_sw; int rc_map_return_sw; + int rc_map_rattitude_sw; int rc_map_posctl_sw; int rc_map_loiter_sw; int rc_map_acro_sw; @@ -295,11 +283,12 @@ class Sensors int rc_map_aux4; int rc_map_aux5; - int rc_map_param[RC_PARAM_MAP_NCHAN]; + int rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; int32_t rc_fails_thr; float rc_assist_th; float rc_auto_th; + float rc_rattitude_th; float rc_posctl_th; float rc_return_th; float rc_loiter_th; @@ -307,6 +296,7 @@ class Sensors float rc_offboard_th; bool rc_assist_inv; bool rc_auto_inv; + bool rc_rattitude_inv; bool rc_posctl_inv; bool rc_return_inv; bool rc_loiter_inv; @@ -338,6 +328,7 @@ class Sensors param_t rc_map_mode_sw; param_t rc_map_return_sw; + param_t rc_map_rattitude_sw; param_t rc_map_posctl_sw; param_t rc_map_loiter_sw; param_t rc_map_acro_sw; @@ -351,8 +342,8 @@ class Sensors param_t rc_map_aux4; param_t rc_map_aux5; - param_t rc_map_param[RC_PARAM_MAP_NCHAN]; - param_t rc_param[RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound + param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; + param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound to a RC channel, equivalent float values in the _parameters struct are not existing because these parameters are never read. */ @@ -360,6 +351,7 @@ class Sensors param_t rc_fails_thr; param_t rc_assist_th; param_t rc_auto_th; + param_t rc_rattitude_th; param_t rc_posctl_th; param_t rc_return_th; param_t rc_loiter_th; @@ -379,6 +371,9 @@ class Sensors } _parameter_handles; /**< handles for interesting parameters */ + int init_sensor_class(const struct orb_metadata *meta, int *subs, + uint32_t *priorities, uint32_t *errcount); + /** * Update our local parameter cache. */ @@ -497,20 +492,18 @@ Sensors::Sensors() : _sensors_task(-1), _hil_enabled(false), _publishing(true), + _armed(false), /* subscriptions */ - _gyro_sub(-1), - _accel_sub(-1), - _mag_sub(-1), - _gyro1_sub(-1), - _accel1_sub(-1), - _mag1_sub(-1), - _gyro2_sub(-1), - _accel2_sub(-1), - _mag2_sub(-1), + _gyro_sub{ -1, -1, -1}, + _accel_sub{ -1, -1, -1}, + _mag_sub{ -1, -1, -1}, + _baro_sub{ -1, -1, -1}, + _gyro_count(0), + _accel_count(0), + _mag_count(0), + _baro_count(0), _rc_sub(-1), - _baro_sub(-1), - _baro1_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), _rc_parameter_map_sub(-1), @@ -535,6 +528,14 @@ Sensors::Sensors() : _battery_discharged(0), _battery_current_timestamp(0) { + /* initialize subscriptions */ + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { + _gyro_sub[i] = -1; + _accel_sub[i] = -1; + _mag_sub[i] = -1; + _baro_sub[i] = -1; + } + memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); @@ -579,6 +580,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_rattitude_sw = param_find("RC_MAP_RATT_SW"); _parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); _parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); @@ -591,9 +593,10 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); /* RC to parameter mapping for changing parameters with RC */ - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - char name[PARAM_ID_LEN]; - snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + char name[rc_parameter_map_s::PARAM_ID_LEN]; + snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", + i + 1); // shifted by 1 because param name starts at 1 _parameter_handles.rc_map_param[i] = param_find(name); } @@ -601,6 +604,7 @@ Sensors::Sensors() : _parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); _parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); _parameter_handles.rc_auto_th = param_find("RC_AUTO_TH"); + _parameter_handles.rc_rattitude_th = param_find("RC_RATT_TH"); _parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); _parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); _parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); @@ -638,7 +642,15 @@ Sensors::Sensors() : (void)param_find("CAL_MAG2_ROT"); (void)param_find("SYS_PARAM_VER"); (void)param_find("SYS_AUTOSTART"); - + (void)param_find("SYS_AUTOCONFIG"); + (void)param_find("PWM_MIN"); + (void)param_find("PWM_MAX"); + (void)param_find("PWM_DISARMED"); + (void)param_find("PWM_AUX_MIN"); + (void)param_find("PWM_AUX_MAX"); + (void)param_find("PWM_AUX_DISARMED"); + (void)param_find("TRIG_MODE"); + /* fetch initial parameter values */ parameters_update(); } @@ -737,6 +749,10 @@ Sensors::parameters_update() warnx("%s", paramerr); } + if (param_get(_parameter_handles.rc_map_rattitude_sw, &(_parameters.rc_map_rattitude_sw)) != OK) { + warnx("%s", paramerr); + } + if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) { warnx("%s", paramerr); } @@ -763,7 +779,7 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); } @@ -774,6 +790,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th)); _parameters.rc_auto_inv = (_parameters.rc_auto_th < 0); _parameters.rc_auto_th = fabs(_parameters.rc_auto_th); + param_get(_parameter_handles.rc_rattitude_th, &(_parameters.rc_rattitude_th)); + _parameters.rc_rattitude_inv = (_parameters.rc_rattitude_th < 0); + _parameters.rc_rattitude_th = fabs(_parameters.rc_rattitude_th); param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th)); _parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0); _parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th); @@ -798,6 +817,7 @@ Sensors::parameters_update() _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _parameters.rc_map_rattitude_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; @@ -811,7 +831,7 @@ Sensors::parameters_update() _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } @@ -822,6 +842,20 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { warnx("%s", paramerr); + + } else if (_parameters.battery_voltage_scaling < 0.0f) { + /* apply scaling according to defaults if set to default */ + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + _parameters.battery_voltage_scaling = 0.0082f; +#elif CONFIG_ARCH_BOARD_AEROCORE + _parameters.battery_voltage_scaling = 0.0063f; +#elif CONFIG_ARCH_BOARD_PX4FMU_V1 + _parameters.battery_voltage_scaling = 0.00459340659f; +#else + /* ensure a missing default trips a low voltage lockdown */ + _parameters.battery_voltage_scaling = 0.00001f; +#endif } /* scaling of ADC ticks to battery current */ @@ -868,7 +902,7 @@ Sensors::parameters_update() barofd = px4_open(BARO0_DEVICE_PATH, 0); if (barofd < 0) { - warnx("ERROR: no barometer foundon %s", BARO0_DEVICE_PATH); + warnx("ERROR: no barometer found on %s", BARO0_DEVICE_PATH); return ERROR; } else { @@ -1013,258 +1047,134 @@ Sensors::adc_init() void Sensors::accel_poll(struct sensor_combined_s &raw) { - bool accel_updated; - orb_check(_accel_sub, &accel_updated); - - if (accel_updated) { - struct accel_report accel_report; - - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; - - raw.accelerometer_m_s2[0] = vect(0); - raw.accelerometer_m_s2[1] = vect(1); - raw.accelerometer_m_s2[2] = vect(2); + for (unsigned i = 0; i < _accel_count; i++) { + bool accel_updated; + orb_check(_accel_sub[i], &accel_updated); - raw.accelerometer_raw[0] = accel_report.x_raw; - raw.accelerometer_raw[1] = accel_report.y_raw; - raw.accelerometer_raw[2] = accel_report.z_raw; + if (accel_updated) { + struct accel_report accel_report; - raw.accelerometer_timestamp = accel_report.timestamp; - raw.accelerometer_errcount = accel_report.error_count; - raw.accelerometer_temp = accel_report.temperature; - } - - orb_check(_accel1_sub, &accel_updated); - - if (accel_updated) { - struct accel_report accel_report; + orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report); - orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report); + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; + raw.accelerometer_m_s2[i * 3 + 0] = vect(0); + raw.accelerometer_m_s2[i * 3 + 1] = vect(1); + raw.accelerometer_m_s2[i * 3 + 2] = vect(2); - raw.accelerometer1_m_s2[0] = vect(0); - raw.accelerometer1_m_s2[1] = vect(1); - raw.accelerometer1_m_s2[2] = vect(2); + math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral); + vect_int = _board_rotation * vect_int; - raw.accelerometer1_raw[0] = accel_report.x_raw; - raw.accelerometer1_raw[1] = accel_report.y_raw; - raw.accelerometer1_raw[2] = accel_report.z_raw; - - raw.accelerometer1_timestamp = accel_report.timestamp; - raw.accelerometer1_errcount = accel_report.error_count; - raw.accelerometer1_temp = accel_report.temperature; - } + raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0); + raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1); + raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2); - orb_check(_accel2_sub, &accel_updated); + raw.accelerometer_integral_dt[i] = accel_report.integral_dt; - if (accel_updated) { - struct accel_report accel_report; + raw.accelerometer_raw[i * 3 + 0] = accel_report.x_raw; + raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw; + raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw; - orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report); - - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; - - raw.accelerometer2_m_s2[0] = vect(0); - raw.accelerometer2_m_s2[1] = vect(1); - raw.accelerometer2_m_s2[2] = vect(2); - - raw.accelerometer2_raw[0] = accel_report.x_raw; - raw.accelerometer2_raw[1] = accel_report.y_raw; - raw.accelerometer2_raw[2] = accel_report.z_raw; - - raw.accelerometer2_timestamp = accel_report.timestamp; - raw.accelerometer2_errcount = accel_report.error_count; - raw.accelerometer2_temp = accel_report.temperature; + raw.accelerometer_timestamp[i] = accel_report.timestamp; + raw.accelerometer_errcount[i] = accel_report.error_count; + raw.accelerometer_temp[i] = accel_report.temperature; + } } } void Sensors::gyro_poll(struct sensor_combined_s &raw) { - bool gyro_updated; - orb_check(_gyro_sub, &gyro_updated); + for (unsigned i = 0; i < _gyro_count; i++) { + bool gyro_updated; + orb_check(_gyro_sub[i], &gyro_updated); - if (gyro_updated) { - struct gyro_report gyro_report; + if (gyro_updated) { + struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report); - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro_rad_s[0] = vect(0); - raw.gyro_rad_s[1] = vect(1); - raw.gyro_rad_s[2] = vect(2); - - raw.gyro_raw[0] = gyro_report.x_raw; - raw.gyro_raw[1] = gyro_report.y_raw; - raw.gyro_raw[2] = gyro_report.z_raw; - - raw.timestamp = gyro_report.timestamp; - raw.gyro_errcount = gyro_report.error_count; - raw.gyro_temp = gyro_report.temperature; - } + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; - orb_check(_gyro1_sub, &gyro_updated); + raw.gyro_rad_s[i * 3 + 0] = vect(0); + raw.gyro_rad_s[i * 3 + 1] = vect(1); + raw.gyro_rad_s[i * 3 + 2] = vect(2); - if (gyro_updated) { - struct gyro_report gyro_report; + math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral); + vect_int = _board_rotation * vect_int; - orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report); + raw.gyro_integral_rad[i * 3 + 0] = vect_int(0); + raw.gyro_integral_rad[i * 3 + 1] = vect_int(1); + raw.gyro_integral_rad[i * 3 + 2] = vect_int(2); - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; + raw.gyro_integral_dt[i] = gyro_report.integral_dt; - raw.gyro1_rad_s[0] = vect(0); - raw.gyro1_rad_s[1] = vect(1); - raw.gyro1_rad_s[2] = vect(2); + raw.gyro_raw[i * 3 + 0] = gyro_report.x_raw; + raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw; + raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw; - raw.gyro1_raw[0] = gyro_report.x_raw; - raw.gyro1_raw[1] = gyro_report.y_raw; - raw.gyro1_raw[2] = gyro_report.z_raw; + raw.gyro_timestamp[i] = gyro_report.timestamp; - raw.gyro1_timestamp = gyro_report.timestamp; - raw.gyro1_errcount = gyro_report.error_count; - raw.gyro1_temp = gyro_report.temperature; - } - - orb_check(_gyro2_sub, &gyro_updated); - - if (gyro_updated) { - struct gyro_report gyro_report; - - orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report); - - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro2_rad_s[0] = vect(0); - raw.gyro2_rad_s[1] = vect(1); - raw.gyro2_rad_s[2] = vect(2); - - raw.gyro2_raw[0] = gyro_report.x_raw; - raw.gyro2_raw[1] = gyro_report.y_raw; - raw.gyro2_raw[2] = gyro_report.z_raw; + if (i == 0) { + raw.timestamp = gyro_report.timestamp; + } - raw.gyro2_timestamp = gyro_report.timestamp; - raw.gyro2_errcount = gyro_report.error_count; - raw.gyro2_temp = gyro_report.temperature; + raw.gyro_errcount[i] = gyro_report.error_count; + raw.gyro_temp[i] = gyro_report.temperature; + } } } void Sensors::mag_poll(struct sensor_combined_s &raw) { - bool mag_updated; - orb_check(_mag_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; - - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - - vect = _mag_rotation[0] * vect; - - raw.magnetometer_ga[0] = vect(0); - raw.magnetometer_ga[1] = vect(1); - raw.magnetometer_ga[2] = vect(2); - - raw.magnetometer_raw[0] = mag_report.x_raw; - raw.magnetometer_raw[1] = mag_report.y_raw; - raw.magnetometer_raw[2] = mag_report.z_raw; - - raw.magnetometer_timestamp = mag_report.timestamp; - raw.magnetometer_errcount = mag_report.error_count; - raw.magnetometer_temp = mag_report.temperature; - } - - orb_check(_mag1_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; + for (unsigned i = 0; i < _mag_count; i++) { + bool mag_updated; + orb_check(_mag_sub[i], &mag_updated); - orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); + if (mag_updated) { + struct mag_report mag_report; - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report); - vect = _mag_rotation[1] * vect; + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - raw.magnetometer1_ga[0] = vect(0); - raw.magnetometer1_ga[1] = vect(1); - raw.magnetometer1_ga[2] = vect(2); + vect = _mag_rotation[i] * vect; - raw.magnetometer1_raw[0] = mag_report.x_raw; - raw.magnetometer1_raw[1] = mag_report.y_raw; - raw.magnetometer1_raw[2] = mag_report.z_raw; + raw.magnetometer_ga[i * 3 + 0] = vect(0); + raw.magnetometer_ga[i * 3 + 1] = vect(1); + raw.magnetometer_ga[i * 3 + 2] = vect(2); - raw.magnetometer1_timestamp = mag_report.timestamp; - raw.magnetometer1_errcount = mag_report.error_count; - raw.magnetometer1_temp = mag_report.temperature; - } - - orb_check(_mag2_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; - - orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); - - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + raw.magnetometer_raw[i * 3 + 0] = mag_report.x_raw; + raw.magnetometer_raw[i * 3 + 1] = mag_report.y_raw; + raw.magnetometer_raw[i * 3 + 2] = mag_report.z_raw; - vect = _mag_rotation[2] * vect; - - raw.magnetometer2_ga[0] = vect(0); - raw.magnetometer2_ga[1] = vect(1); - raw.magnetometer2_ga[2] = vect(2); - - raw.magnetometer2_raw[0] = mag_report.x_raw; - raw.magnetometer2_raw[1] = mag_report.y_raw; - raw.magnetometer2_raw[2] = mag_report.z_raw; - - raw.magnetometer2_timestamp = mag_report.timestamp; - raw.magnetometer2_errcount = mag_report.error_count; - raw.magnetometer2_temp = mag_report.temperature; + raw.magnetometer_timestamp[i] = mag_report.timestamp; + raw.magnetometer_errcount[i] = mag_report.error_count; + raw.magnetometer_temp[i] = mag_report.temperature; + } } } void Sensors::baro_poll(struct sensor_combined_s &raw) { - bool baro_updated; - orb_check(_baro_sub, &baro_updated); + for (unsigned i = 0; i < _baro_count; i++) { + bool baro_updated; + orb_check(_baro_sub[i], &baro_updated); - if (baro_updated) { + if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); + orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer); - raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar - raw.baro_alt_meter = _barometer.altitude; // Altitude in meters - raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius + raw.baro_pres_mbar[i] = _barometer.pressure; // Pressure in mbar + raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters + raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius - raw.baro_timestamp = _barometer.timestamp; - } - - orb_check(_baro1_sub, &baro_updated); - - if (baro_updated) { - - struct baro_report baro_report; - - orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report); - - raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar - raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters - raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius - - raw.baro1_timestamp = baro_report.timestamp; + raw.baro_timestamp[i] = _barometer.timestamp; + } } } @@ -1277,12 +1187,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) if (updated) { orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); - raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa; - raw.differential_pressure_timestamp = _diff_pres.timestamp; - raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; + raw.differential_pressure_pa[0] = _diff_pres.differential_pressure_raw_pa; + raw.differential_pressure_timestamp[0] = _diff_pres.timestamp; + raw.differential_pressure_filtered_pa[0] = _diff_pres.differential_pressure_filtered_pa; float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : - (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + (raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; @@ -1290,8 +1200,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); _airspeed.true_airspeed_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f, + raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); + _airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, + calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f, + raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); + _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ @@ -1318,16 +1232,17 @@ Sensors::vehicle_control_mode_poll() orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); /* switching from non-HIL to HIL mode */ - //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { _hil_enabled = true; _publishing = false; + _armed = vcontrol_mode.flag_armed; /* switching from HIL to non-HIL mode */ } else if (!_publishing && !_hil_enabled) { _hil_enabled = false; _publishing = true; + _armed = vcontrol_mode.flag_armed; } } } @@ -1335,7 +1250,7 @@ Sensors::vehicle_control_mode_poll() void Sensors::parameter_update_poll(bool forced) { - bool param_updated; + bool param_updated = false; /* Check if any parameter has changed */ orb_check(_params_sub, ¶m_updated); @@ -1357,7 +1272,7 @@ Sensors::parameter_update_poll(bool forced) unsigned accel_count = 0; /* run through all gyro sensors */ - for (unsigned s = 0; s < 3; s++) { + for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { res = ERROR; (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); @@ -1370,8 +1285,8 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; - /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { + /* run through all stored calibrations */ + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1402,15 +1317,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); + } else { config_ok = true; } } + break; } } @@ -1423,7 +1342,7 @@ Sensors::parameter_update_poll(bool forced) } /* run through all accel sensors */ - for (unsigned s = 0; s < 3; s++) { + for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { res = ERROR; (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); @@ -1436,8 +1355,8 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; - /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { + /* run through all stored calibrations */ + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1468,15 +1387,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); + } else { config_ok = true; } } + break; } } @@ -1489,7 +1412,13 @@ Sensors::parameter_update_poll(bool forced) } /* run through all mag sensors */ - for (unsigned s = 0; s < 3; s++) { + for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { + + /* set a valid default rotation (same as board). + * if the mag is configured, this might be replaced + * in the section below. + */ + _mag_rotation[s] = _board_rotation; res = ERROR; (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); @@ -1501,22 +1430,18 @@ Sensors::parameter_update_poll(bool forced) continue; } - /* set a valid default rotation (same as board). - * if the mag is configured, this might be replaced - * in the section below. - */ - _mag_rotation[s] = _board_rotation; - bool config_ok = false; - /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { + /* run through all stored calibrations */ + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; (void)sprintf(str, "CAL_MAG%u_ID", i); int device_id; failed = failed || (OK != param_get(param_find(str), &device_id)); + (void)sprintf(str, "CAL_MAG%u_ROT", i); + (void)param_find(str); if (failed) { px4_close(fd); @@ -1547,6 +1472,7 @@ Sensors::parameter_update_poll(bool forced) /* reset param to -1 to indicate internal mag */ int32_t minus_one = MAG_ROT_VAL_INTERNAL; param_set_no_notification(param_find(str), &minus_one); + } else { int32_t mag_rot; @@ -1591,15 +1517,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); + } else { config_ok = true; } } + break; } } @@ -1643,7 +1573,7 @@ Sensors::rc_parameter_map_poll(bool forced) orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); /* update paramter handles to which the RC channels are mapped */ - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink @@ -1656,17 +1586,17 @@ Sensors::rc_parameter_map_poll(bool forced) _parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]); } else { - _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); + _parameter_handles.rc_param[i] = param_find(&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]); } } warnx("rc to parameter map updated"); - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", i, - _rc_parameter_map.param_id[i], + &_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)], (double)_rc_parameter_map.scale[i], (double)_rc_parameter_map.value0[i], (double)_rc_parameter_map.value_min[i], @@ -1858,7 +1788,7 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) void Sensors::set_params_from_rc() { - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink @@ -1867,6 +1797,7 @@ Sensors::set_params_from_rc() } float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); + /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ if (rc_val > _param_rc_values[i] + FLT_EPSILON || rc_val < _param_rc_values[i] - FLT_EPSILON) { @@ -1998,24 +1929,33 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); - manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); - manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); - manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); + manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, + _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, + _parameters.rc_rattitude_th, + _parameters.rc_rattitude_inv); + manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, + _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, + _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, + _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, + _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, + _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub != nullptr) { @@ -2065,51 +2005,109 @@ Sensors::task_main_trampoline(int argc, char *argv[]) sensors::g_sensors->task_main(); } +int +Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, + uint32_t *priorities, uint32_t *errcount) +{ + unsigned group_count = orb_group_count(meta); + + if (group_count > SENSOR_COUNT_MAX) { + group_count = SENSOR_COUNT_MAX; + } + + for (unsigned i = 0; i < group_count; i++) { + if (subs[i] < 0) { + subs[i] = orb_subscribe_multi(meta, i); + orb_priority(subs[i], (int32_t *)&priorities[i]); + } + } + + return group_count; +} + void Sensors::task_main() { /* start individual sensors */ int ret = 0; + do { /* create a scope to handle exit with break */ ret = accel_init(); - if (ret) break; + + if (ret) { break; } + ret = gyro_init(); - if (ret) break; + + if (ret) { break; } + ret = mag_init(); - if (ret) break; + + if (ret) { break; } + ret = baro_init(); - if (ret) break; + + if (ret) { break; } + ret = adc_init(); - if (ret) break; + + if (ret) { break; } + break; } while (0); if (ret) { - warnx("Sensor initialization failed"); + warnx("sensor initialization failed"); _sensors_task = -1; - if (_fd_adc >=0) { - close(_fd_adc); + + if (_fd_adc >= 0) { + px4_close(_fd_adc); _fd_adc = -1; } + return; } + struct sensor_combined_s raw = {}; + + /* ensure no overflows can occur */ + static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX, + "SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur"); + /* * do subscriptions */ - _gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); - _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); - _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); - _gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); - _accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); - _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); - _gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2); - _accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2); - _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); + + unsigned gcount_prev = _gyro_count; + + unsigned mcount_prev = _mag_count; + + unsigned acount_prev = _accel_count; + + unsigned bcount_prev = _baro_count; + + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], + &raw.gyro_priority[0], &raw.gyro_errcount[0]); + + _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], + &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); + + _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], + &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); + + _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], + &raw.baro_priority[0], &raw.baro_errcount[0]); + + if (gcount_prev != _gyro_count || + mcount_prev != _mag_count || + acount_prev != _accel_count || + bcount_prev != _baro_count) { + + /* reload calibration params */ + parameter_update_poll(true); + } + _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); - _baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -2119,35 +2117,15 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ - orb_set_interval(_gyro_sub, 4); - /* * do advertisements */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); raw.adc_voltage_v[0] = 0.0f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; raw.adc_voltage_v[3] = 0.0f; - /* set high initial error counts to deselect gyros */ - raw.gyro_errcount = 100000; - raw.gyro1_errcount = 100000; - raw.gyro2_errcount = 100000; - - /* set high initial error counts to deselect accels */ - raw.accelerometer_errcount = 100000; - raw.accelerometer1_errcount = 100000; - raw.accelerometer2_errcount = 100000; - - /* set high initial error counts to deselect mags */ - raw.magnetometer_errcount = 100000; - raw.magnetometer1_errcount = 100000; - raw.magnetometer2_errcount = 100000; - memset(&_battery_status, 0, sizeof(_battery_status)); _battery_status.voltage_v = -1.0f; _battery_status.voltage_filtered_v = -1.0f; @@ -2168,14 +2146,18 @@ Sensors::task_main() _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); /* wakeup source(s) */ - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] = {}; - /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ - fds[0].fd = _gyro_sub; + /* use the gyro to pace output */ + fds[0].fd = _gyro_sub[0]; fds[0].events = POLLIN; _task_should_exit = false; + raw.timestamp = 0; + + uint64_t _last_config_update = hrt_absolute_time(); + while (!_task_should_exit) { /* wait for up to 50ms for data */ @@ -2185,7 +2167,7 @@ Sensors::task_main() /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { - warn("poll error %d, %d", pret, errno); + warnx("poll error %d, %d", pret, errno); continue; } @@ -2202,13 +2184,14 @@ Sensors::task_main() baro_poll(raw); /* work out if main gyro timed out and fail over to alternate gyro */ - if (hrt_elapsed_time(&raw.timestamp) > 20 * 1000) { + if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000) { /* if the secondary failed as well, go to the tertiary */ - if (hrt_elapsed_time(&raw.gyro1_timestamp) > 20 * 1000) { - fds[0].fd = _gyro2_sub; + if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) { + fds[0].fd = _gyro_sub[2]; + } else { - fds[0].fd = _gyro1_sub; + fds[0].fd = _gyro_sub[1]; } } @@ -2218,15 +2201,36 @@ Sensors::task_main() diff_pres_poll(raw); /* Inform other processes that new data is available to copy */ - if (_publishing) { + if (_publishing && raw.timestamp > 0) { orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); } - /* check parameters for updates */ - parameter_update_poll(); + /* keep adding sensors as long as we are not armed, + * when not adding sensors poll for param updates + */ + if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) { + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], + &raw.gyro_priority[0], &raw.gyro_errcount[0]); + + _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], + &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); + + _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], + &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); + + _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], + &raw.baro_priority[0], &raw.baro_errcount[0]); + + _last_config_update = hrt_absolute_time(); - /* check rc parameter map for updates */ - rc_parameter_map_poll(); + } else { + + /* check parameters for updates */ + parameter_update_poll(); + + /* check rc parameter map for updates */ + rc_parameter_map_poll(); + } /* Look for new r/c input data */ rc_poll(); @@ -2245,12 +2249,12 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = px4_task_spawn_cmd("sensors_task", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - (px4_main_t)&Sensors::task_main_trampoline, - nullptr); + _sensors_task = px4_task_spawn_cmd("sensors", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + (px4_main_t)&Sensors::task_main_trampoline, + nullptr); /* wait until the task is up and running or has failed */ while (_sensors_task > 0 && _task_should_exit) { diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt new file mode 100644 index 000000000000..0a7e2210860b --- /dev/null +++ b/src/modules/simulator/CMakeLists.txt @@ -0,0 +1,54 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +set(SIMULATOR_SRCS simulator.cpp) +if (NOT ${OS} STREQUAL "qurt") + list(APPEND SIMULATOR_SRCS + simulator_mavlink.cpp) +endif() + +px4_add_module( + MODULE modules__simulator + MAIN simulator + COMPILE_FLAGS + -Wno-attributes + -Wno-packed + -Wno-packed + SRCS + ${SIMULATOR_SRCS} + DEPENDS + platforms__common + git_jmavsim + git_gazebo + ) + +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index f51439faff82..1656a3451fb0 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include "simulator.h" @@ -63,9 +63,6 @@ Simulator *Simulator::getInstance() bool Simulator::getMPUReport(uint8_t *buf, int len) { - // Reads are paced from reading gyrosim and if - // we don't delay here we read too fast - usleep(50000); return _mpu.copyData(buf, len); } @@ -74,96 +71,82 @@ bool Simulator::getRawAccelReport(uint8_t *buf, int len) return _accel.copyData(buf, len); } +bool Simulator::getMagReport(uint8_t *buf, int len) +{ + return _mag.copyData(buf, len); +} + bool Simulator::getBaroSample(uint8_t *buf, int len) { return _baro.copyData(buf, len); } +bool Simulator::getGPSSample(uint8_t *buf, int len) +{ + return _gps.copyData(buf, len); +} + +bool Simulator::getAirspeedSample(uint8_t *buf, int len) +{ + return _airspeed.copyData(buf, len); +} + +void Simulator::write_MPU_data(void *buf) +{ + _mpu.writeData(buf); +} + +void Simulator::write_accel_data(void *buf) +{ + _accel.writeData(buf); +} + +void Simulator::write_mag_data(void *buf) +{ + _mag.writeData(buf); +} + +void Simulator::write_baro_data(void *buf) +{ + _baro.writeData(buf); +} + +void Simulator::write_gps_data(void *buf) +{ + _gps.writeData(buf); +} + +void Simulator::write_airspeed_data(void *buf) +{ + _airspeed.writeData(buf); +} + int Simulator::start(int argc, char *argv[]) { int ret = 0; _instance = new Simulator(); + if (_instance) { - PX4_INFO("Simulator started"); drv_led_start(); + if (argv[2][1] == 's') { + _instance->initializeSensorData(); #ifndef __PX4_QURT - _instance->updateSamples(); + // Update sensor data + _instance->pollForMAVLinkMessages(false); #endif + } else { - _instance->publishSensorsCombined(); + // Update sensor data + _instance->pollForMAVLinkMessages(true); } - } - else { + + } else { PX4_WARN("Simulator creation failed"); ret = 1; } - return ret; -} -void Simulator::publishSensorsCombined() { - - struct baro_report baro; - memset(&baro,0,sizeof(baro)); - baro.pressure = 120000.0f; - - // acceleration report - struct accel_report accel; - memset(&accel,0,sizeof(accel)); - accel.z = 9.81f; - accel.range_m_s2 = 80.0f; - - // gyro report - struct gyro_report gyro; - memset(&gyro, 0 ,sizeof(gyro)); - - // mag report - struct mag_report mag; - memset(&mag, 0 ,sizeof(mag)); - // init publishers - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - - struct sensor_combined_s sensors; - memset(&sensors, 0, sizeof(sensors)); - // fill sensors with some data - sensors.accelerometer_m_s2[2] = 9.81f; - sensors.magnetometer_ga[0] = 0.2f; - sensors.timestamp = hrt_absolute_time(); - sensors.accelerometer_timestamp = hrt_absolute_time(); - sensors.magnetometer_timestamp = hrt_absolute_time(); - sensors.baro_timestamp = hrt_absolute_time(); - // advertise - _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &sensors); - - hrt_abstime time_last = hrt_absolute_time(); - uint64_t delta; - for(;;) { - delta = hrt_absolute_time() - time_last; - if(delta > (uint64_t)1000000) { - time_last = hrt_absolute_time(); - sensors.timestamp = time_last; - sensors.accelerometer_timestamp = time_last; - sensors.magnetometer_timestamp = time_last; - sensors.baro_timestamp = time_last; - baro.timestamp = time_last; - accel.timestamp = time_last; - gyro.timestamp = time_last; - mag.timestamp = time_last; - // publish the sensor values - //PX4_DEBUG("Publishing SensorsCombined\n"); - orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &sensors); - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } - else { - usleep(1000000-delta); - } - } + return ret; } static void usage() @@ -175,87 +158,58 @@ static void usage() __BEGIN_DECLS extern int simulator_main(int argc, char *argv[]); -extern void led_init(void); -extern void led_on(int led); -extern void led_off(int led); -extern void led_toggle(int led); __END_DECLS extern "C" { -int simulator_main(int argc, char *argv[]) -{ - int ret = 0; - if (argc == 3 && strcmp(argv[1], "start") == 0) { - if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) { - if (g_sim_task >= 0) { - warnx("Simulator already started"); - return 0; + int simulator_main(int argc, char *argv[]) + { + int ret = 0; + + if (argc == 3 && strcmp(argv[1], "start") == 0) { + if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) { + if (g_sim_task >= 0) { + warnx("Simulator already started"); + return 0; + } + + g_sim_task = px4_task_spawn_cmd("Simulator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + Simulator::start, + argv); + + // now wait for the command to complete + while (true) { + if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) { + break; + + } else { + usleep(100000); + } + } + + } else { + usage(); + ret = -EINVAL; } - g_sim_task = px4_task_spawn_cmd("Simulator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - Simulator::start, - argv); - } - else - { - usage(); - ret = -EINVAL; - } - } - else if (argc == 2 && strcmp(argv[1], "stop") == 0) { - if (g_sim_task < 0) { - PX4_WARN("Simulator not running"); - } - else { - px4_task_delete(g_sim_task); - g_sim_task = -1; - } - } - else { - usage(); - ret = -EINVAL; - } - return ret; -} + } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { + if (g_sim_task < 0) { + PX4_WARN("Simulator not running"); -} - -bool static _led_state[2] = { false , false }; - -__EXPORT void led_init() -{ - PX4_DEBUG("LED_INIT"); -} + } else { + px4_task_delete(g_sim_task); + g_sim_task = -1; + } -__EXPORT void led_on(int led) -{ - if (led == 1 || led == 0) - { - PX4_DEBUG("LED%d_ON", led); - _led_state[led] = true; - } -} + } else { + usage(); + ret = -EINVAL; + } -__EXPORT void led_off(int led) -{ - if (led == 1 || led == 0) - { - PX4_DEBUG("LED%d_OFF", led); - _led_state[led] = false; + return ret; } -} -__EXPORT void led_toggle(int led) -{ - if (led == 1 || led == 0) - { - _led_state[led] = !_led_state[led]; - PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF"); - - } } - diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index a95fa157311d..6f9c35bb9555 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -38,58 +38,97 @@ #pragma once -#include -#include +#include +#include #include #include #include +#include #include #include #include #include #include +#include #include +#include #include #include -#ifndef __PX4_QURT -#include -#include -#endif - -namespace simulator { +namespace simulator +{ // FIXME - what is the endianness of these on actual device? #pragma pack(push, 1) struct RawAccelData { - int16_t x; - int16_t y; - int16_t z; + float temperature; + float x; + float y; + float z; +}; +#pragma pack(pop) + +#pragma pack(push, 1) +struct RawMagData { + float temperature; + float x; + float y; + float z; }; #pragma pack(pop) #pragma pack(push, 1) struct RawMPUData { - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; + float accel_x; + float accel_y; + float accel_z; + float temp; + float gyro_x; + float gyro_y; + float gyro_z; }; #pragma pack(pop) +#pragma pack(push, 1) struct RawBaroData { - uint8_t d[3]; + float pressure; + float altitude; + float temperature; }; +#pragma pack(pop) -template class Report { +#pragma pack(push, 1) +struct RawAirspeedData { + float temperature; + float diff_pressure; +}; +#pragma pack(pop) + +#pragma pack(push, 1) +struct RawGPSData { + int32_t lat; + int32_t lon; + int32_t alt; + uint16_t eph; + uint16_t epv; + uint16_t vel; + int16_t vn; + int16_t ve; + int16_t vd; + uint16_t cog; + uint8_t fix_type; + uint8_t satellites_visible; +}; +#pragma pack(pop) + +template class Report +{ public: Report(int readers) : - _max_readers(readers), - _report_len(sizeof(RType)) + _readidx(0), + _max_readers(readers), + _report_len(sizeof(RType)) { - sem_init(&_lock, 0, _max_readers); + px4_sem_init(&_lock, 0, _max_readers); } ~Report() {}; @@ -99,6 +138,7 @@ template class Report { if (len != _report_len) { return false; } + read_lock(); memcpy(outbuf, &_buf[_readidx], _report_len); read_unlock(); @@ -113,23 +153,23 @@ template class Report { } protected: - void read_lock() { sem_wait(&_lock); } - void read_unlock() { sem_post(&_lock); } - void write_lock() + void read_lock() { px4_sem_wait(&_lock); } + void read_unlock() { px4_sem_post(&_lock); } + void write_lock() { - for (int i=0; i<_max_readers; i++) { - sem_wait(&_lock); - } + for (int i = 0; i < _max_readers; i++) { + px4_sem_wait(&_lock); + } } void write_unlock() { - for (int i=0; i<_max_readers; i++) { - sem_post(&_lock); + for (int i = 0; i < _max_readers; i++) { + px4_sem_post(&_lock); } } int _readidx; - sem_t _lock; + px4_sem_t _lock; const int _max_readers; const int _report_len; RType _buf[2]; @@ -137,7 +177,8 @@ template class Report { }; -class Simulator { +class Simulator +{ public: static Simulator *getInstance(); @@ -158,73 +199,101 @@ class Simulator { static int start(int argc, char *argv[]); bool getRawAccelReport(uint8_t *buf, int len); + bool getMagReport(uint8_t *buf, int len); bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); + bool getGPSSample(uint8_t *buf, int len); + bool getAirspeedSample(uint8_t *buf, int len); + + void write_MPU_data(void *buf); + void write_accel_data(void *buf); + void write_mag_data(void *buf); + void write_baro_data(void *buf); + void write_gps_data(void *buf); + void write_airspeed_data(void *buf); + + bool isInitialized() { return _initialized; } + private: Simulator() : - _accel(1), - _mpu(1), - _baro(1), - _sensor_combined_pub(nullptr) + _accel(1), + _mpu(1), + _baro(1), + _mag(1), + _gps(1), + _airspeed(1), + _accel_pub(nullptr), + _baro_pub(nullptr), + _gyro_pub(nullptr), + _mag_pub(nullptr), + _initialized(false) #ifndef __PX4_QURT - , - _manual_control_sp_pub(nullptr), - _actuator_outputs_sub(-1), - _vehicle_attitude_sub(-1), - _sensor{}, - _manual_control_sp{}, - _actuators{}, - _attitude{} + , + _rc_channels_pub(nullptr), + _actuator_outputs_sub(-1), + _vehicle_attitude_sub(-1), + _manual_sub(-1), + _vehicle_status_sub(-1), + _rc_input{}, + _actuators{}, + _attitude{}, + _manual{}, + _vehicle_status{} #endif {} - ~Simulator() { _instance=NULL; } + ~Simulator() { _instance = NULL; } -#ifndef __PX4_QURT - void updateSamples(); -#endif + void initializeSensorData(); static Simulator *_instance; // simulated sensor instances - simulator::Report _accel; + simulator::Report _accel; simulator::Report _mpu; simulator::Report _baro; + simulator::Report _mag; + simulator::Report _gps; + simulator::Report _airspeed; // uORB publisher handlers orb_advert_t _accel_pub; orb_advert_t _baro_pub; orb_advert_t _gyro_pub; orb_advert_t _mag_pub; - orb_advert_t _sensor_combined_pub; + orb_advert_t _flow_pub; + + bool _initialized; // class methods - void publishSensorsCombined(); + int publish_sensor_topics(mavlink_hil_sensor_t *imu); + int publish_flow_topic(mavlink_hil_optical_flow_t *flow); #ifndef __PX4_QURT // uORB publisher handlers - orb_advert_t _manual_control_sp_pub; + orb_advert_t _rc_channels_pub; // uORB subscription handlers int _actuator_outputs_sub; int _vehicle_attitude_sub; + int _manual_sub; + int _vehicle_status_sub; // uORB data containers - struct sensor_combined_s _sensor; - struct manual_control_setpoint_s _manual_control_sp; + struct rc_input_values _rc_input; struct actuator_outputs_s _actuators; struct vehicle_attitude_s _attitude; - - int _fd; - unsigned char _buf[200]; - hrt_abstime _time_last; - struct sockaddr_in _srcaddr; - socklen_t _addrlen = sizeof(_srcaddr); + struct manual_control_setpoint_s _manual; + struct vehicle_status_s _vehicle_status; void poll_topics(); - void handle_message(mavlink_message_t *msg); - void send_data(); + void handle_message(mavlink_message_t *msg, bool publish); + void send_controls(); + void pollForMAVLinkMessages(bool publish); + void pack_actuator_message(mavlink_hil_controls_t &actuator_msg); void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); + void update_sensors(mavlink_hil_sensor_t *imu); + void update_gps(mavlink_hil_gps_t *gps_sim); static void *sending_trampoline(void *); void send(); #endif diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 519c69332824..2a6efc7fa73b 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -30,50 +30,90 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - +#include #include #include #include "simulator.h" #include "errno.h" +#include #include +#include +#include -using namespace simulator; +extern "C" __EXPORT hrt_abstime hrt_reset(void); #define SEND_INTERVAL 20 -#define UDP_PORT 14550 +#define UDP_PORT 14560 #define PIXHAWK_DEVICE "/dev/ttyACM0" +#ifndef B460800 +#define B460800 460800 +#endif + +#ifndef B921600 +#define B921600 921600 +#endif + +#define PRESS_GROUND 101325.0f +#define DENSITY 1.2041f +#define GRAVITY 9.81f + static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; +static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; + +static int openUart(const char *uart_name, int baud); -void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { - float out[8]; +static int _fd; +static unsigned char _buf[200]; +sockaddr_in _srcaddr; +static socklen_t _addrlen = sizeof(_srcaddr); + +using namespace simulator; + +void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) +{ + float out[8] = {}; const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; // for now we only support quadrotors unsigned n = 4; - for (unsigned i = 0; i < 8; i++) { - if (_actuators.output[i] > PWM_LOWEST_MIN / 2) { - if (i < n) { - // scale PWM out 900..2100 us to 0..1 for rotors */ - out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + if (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) { + for (unsigned i = 0; i < 8; i++) { + if (_actuators.output[i] > PWM_LOWEST_MIN / 2) { + if (i < n) { + // scale PWM out 900..2100 us to 0..1 for rotors */ + out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + // scale PWM out 900..2100 us to -1..1 for other channels */ + out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + } } else { - // scale PWM out 900..2100 us to -1..1 for other channels */ - out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + // send 0 when disarmed and for disabled channels */ + out[i] = 0.0f; } + } - } else { - // send 0 when disarmed and for disabled channels */ - out[i] = 0.0f; + } else { + // convert back to range [-1, 1] + for (unsigned i = 0; i < 8; i++) { + out[i] = (_actuators.output[i] - 1500) / 600.0f; } } + // if vehicle status has not yet been updated, set actuator commands to zero + // this is to prevent the simulation getting into a bad state + if (_vehicle_status.timestamp == 0) { + memset(out, 0, sizeof(out)); + } + actuator_msg.time_usec = hrt_absolute_time(); actuator_msg.roll_ailerons = out[0]; - actuator_msg.pitch_elevator = out[1]; + actuator_msg.pitch_elevator = (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) ? out[1] : -out[1]; actuator_msg.yaw_rudder = out[2]; actuator_msg.throttle = out[3]; actuator_msg.aux1 = out[4]; @@ -84,103 +124,165 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { actuator_msg.nav_mode = 0; } -void Simulator::send_data() { - // check if it's time to send new data - hrt_abstime time_now = hrt_absolute_time(); - if (true) {//time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) { - _time_last = time_now; - mavlink_hil_controls_t msg; - pack_actuator_message(msg); - send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); - // can add more messages here, can also setup different timings - } +void Simulator::send_controls() +{ + mavlink_hil_controls_t msg; + pack_actuator_message(msg); + send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); } -static void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg) { - manual->timestamp = hrt_absolute_time(); - manual->x = man_msg->x / 1000.0f; - manual->y = man_msg->y / 1000.0f; - manual->r = man_msg->r / 1000.0f; - manual->z = man_msg->z / 1000.0f; +static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) +{ + rc->timestamp_publication = hrt_absolute_time(); + rc->timestamp_last_signal = hrt_absolute_time(); + rc->channel_count = rc_channels->chancount; + rc->rssi = rc_channels->rssi; + + /* PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d", + rc_channels->chan1_raw, + rc_channels->chan2_raw, + rc_channels->chan3_raw, + rc_channels->chan4_raw, + rc_channels->chan5_raw, + rc_channels->chan6_raw, + rc_channels->chan7_raw, + rc_channels->chan8_raw); + */ + + rc->values[0] = rc_channels->chan1_raw; + rc->values[1] = rc_channels->chan2_raw; + rc->values[2] = rc_channels->chan3_raw; + rc->values[3] = rc_channels->chan4_raw; + rc->values[4] = rc_channels->chan5_raw; + rc->values[5] = rc_channels->chan6_raw; + rc->values[6] = rc_channels->chan7_raw; + rc->values[7] = rc_channels->chan8_raw; + rc->values[8] = rc_channels->chan9_raw; + rc->values[9] = rc_channels->chan10_raw; + rc->values[10] = rc_channels->chan11_raw; + rc->values[11] = rc_channels->chan12_raw; + rc->values[12] = rc_channels->chan13_raw; + rc->values[13] = rc_channels->chan14_raw; + rc->values[14] = rc_channels->chan15_raw; + rc->values[15] = rc_channels->chan16_raw; + rc->values[16] = rc_channels->chan17_raw; + rc->values[17] = rc_channels->chan18_raw; } -void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) { - hrt_abstime timestamp = hrt_absolute_time(); - sensor->timestamp = timestamp; - sensor->gyro_raw[0] = imu->xgyro * 1000.0f; - sensor->gyro_raw[1] = imu->ygyro * 1000.0f; - sensor->gyro_raw[2] = imu->zgyro * 1000.0f; - sensor->gyro_rad_s[0] = imu->xgyro; - sensor->gyro_rad_s[1] = imu->ygyro; - sensor->gyro_rad_s[2] = imu->zgyro; - - sensor->accelerometer_raw[0] = imu->xacc; // mg2ms2; - sensor->accelerometer_raw[1] = imu->yacc; // mg2ms2; - sensor->accelerometer_raw[2] = imu->zacc; // mg2ms2; - sensor->accelerometer_m_s2[0] = imu->xacc; - sensor->accelerometer_m_s2[1] = imu->yacc; - sensor->accelerometer_m_s2[2] = imu->zacc; - sensor->accelerometer_mode = 0; // TODO what is this? - sensor->accelerometer_range_m_s2 = 32.7f; // int16 - sensor->accelerometer_timestamp = timestamp; - sensor->timestamp = timestamp; - - sensor->adc_voltage_v[0] = 0.0f; - sensor->adc_voltage_v[1] = 0.0f; - sensor->adc_voltage_v[2] = 0.0f; - - sensor->magnetometer_raw[0] = imu->xmag * 1000.0f; - sensor->magnetometer_raw[1] = imu->ymag * 1000.0f; - sensor->magnetometer_raw[2] = imu->zmag * 1000.0f; - sensor->magnetometer_ga[0] = imu->xmag; - sensor->magnetometer_ga[1] = imu->ymag; - sensor->magnetometer_ga[2] = imu->zmag; - sensor->magnetometer_range_ga = 32.7f; // int16 - sensor->magnetometer_mode = 0; // TODO what is this - sensor->magnetometer_cuttoff_freq_hz = 50.0f; - sensor->magnetometer_timestamp = timestamp; - - sensor->baro_pres_mbar = imu->abs_pressure; - sensor->baro_alt_meter = imu->pressure_alt; - sensor->baro_temp_celcius = imu->temperature; - sensor->baro_timestamp = timestamp; - - sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa - sensor->differential_pressure_timestamp = timestamp; +void Simulator::update_sensors(mavlink_hil_sensor_t *imu) +{ + // write sensor data to memory so that drivers can copy data from there + RawMPUData mpu = {}; + mpu.accel_x = imu->xacc; + mpu.accel_y = imu->yacc; + mpu.accel_z = imu->zacc; + mpu.temp = imu->temperature; + mpu.gyro_x = imu->xgyro; + mpu.gyro_y = imu->ygyro; + mpu.gyro_z = imu->zgyro; + + write_MPU_data((void *)&mpu); + + RawAccelData accel = {}; + accel.x = imu->xacc; + accel.y = imu->yacc; + accel.z = imu->zacc; + + write_accel_data((void *)&accel); + + RawMagData mag = {}; + mag.x = imu->xmag; + mag.y = imu->ymag; + mag.z = imu->zmag; + + write_mag_data((void *)&mag); + + RawBaroData baro = {}; + // calculate air pressure from altitude (valid for low altitude) + baro.pressure = (PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt) / 100.0f; // convert from Pa to mbar + baro.altitude = imu->pressure_alt; + baro.temperature = imu->temperature; + + write_baro_data((void *)&baro); + + RawAirspeedData airspeed = {}; + airspeed.temperature = imu->temperature; + airspeed.diff_pressure = imu->diff_pressure; + + write_airspeed_data((void *)&airspeed); } -void Simulator::handle_message(mavlink_message_t *msg) { - switch(msg->msgid) { - case MAVLINK_MSG_ID_HIL_SENSOR: - mavlink_hil_sensor_t imu; - mavlink_msg_hil_sensor_decode(msg, &imu); - fill_sensors_from_imu_msg(&_sensor, &imu); +void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) +{ + RawGPSData gps; + gps.lat = gps_sim->lat; + gps.lon = gps_sim->lon; + gps.alt = gps_sim->alt; + gps.eph = gps_sim->eph; + gps.epv = gps_sim->epv; + gps.vel = gps_sim->vel; + gps.vn = gps_sim->vn; + gps.ve = gps_sim->ve; + gps.vd = gps_sim->vd; + gps.cog = gps_sim->cog; + gps.fix_type = gps_sim->fix_type; + gps.satellites_visible = gps_sim->satellites_visible; + + write_gps_data((void *)&gps); +} - // publish message - if(_sensor_combined_pub == nullptr) { - _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor); - } else { - orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor); - } - break; +void Simulator::handle_message(mavlink_message_t *msg, bool publish) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_SENSOR: + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); - case MAVLINK_MSG_ID_MANUAL_CONTROL: + if (publish) { + publish_sensor_topics(&imu); + } + + update_sensors(&imu); + break; + + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + mavlink_hil_optical_flow_t flow; + mavlink_msg_hil_optical_flow_decode(msg, &flow); + publish_flow_topic(&flow); + break; + + case MAVLINK_MSG_ID_HIL_GPS: + mavlink_hil_gps_t gps_sim; + mavlink_msg_hil_gps_decode(msg, &gps_sim); + + if (publish) { + //PX4_WARN("FIXME: Need to publish GPS topic. Not done yet."); + } - mavlink_manual_control_t man_ctrl_sp; - mavlink_msg_manual_control_decode(msg, &man_ctrl_sp); - fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp); + update_gps(&gps_sim); + break; + + case MAVLINK_MSG_ID_RC_CHANNELS: + mavlink_rc_channels_t rc_channels; + mavlink_msg_rc_channels_decode(msg, &rc_channels); + fill_rc_input_msg(&_rc_input, &rc_channels); + + // publish message + if (publish) { + if (_rc_channels_pub == nullptr) { + _rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input); - // publish message - if(_manual_control_sp_pub == nullptr) { - _manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp); } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp); + orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input); } - break; + } + + break; } } -void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { +void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) +{ component_ID = 0; uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; @@ -197,7 +299,7 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8 buf[5] = msgid; /* payload */ - memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len); + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); /* checksum */ uint16_t checksum; @@ -209,44 +311,51 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8 buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); ssize_t len = sendto(_fd, buf, packet_len, 0, (struct sockaddr *)&_srcaddr, _addrlen); + if (len <= 0) { PX4_WARN("Failed sending mavlink message"); } } -void Simulator::poll_topics() { - // copy new data if available +void Simulator::poll_topics() +{ + // copy new actuator data if available bool updated; orb_check(_actuator_outputs_sub, &updated); - if(updated) { + + if (updated) { orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators); } - orb_check(_vehicle_attitude_sub, &updated); - if(updated) { - orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude); + orb_check(_vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); } } -void *Simulator::sending_trampoline(void *) { +void *Simulator::sending_trampoline(void *) +{ _instance->send(); return 0; // why do I have to put this??? } -void Simulator::send() { +void Simulator::send() +{ px4_pollfd_struct_t fds[1]; fds[0].fd = _actuator_outputs_sub; fds[0].events = POLLIN; - _time_last = hrt_absolute_time(); + int pret; - while(true) { + while (true) { // wait for up to 100ms for data - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - //timed out - if (pret == 0) + // timed out + if (pret == 0) { continue; + } // this is undesirable but not much we can do if (pret < 0) { @@ -259,44 +368,49 @@ void Simulator::send() { if (fds[0].revents & POLLIN) { // got new data to read, update all topics poll_topics(); - send_data(); + send_controls(); } } } -void Simulator::updateSamples() +void Simulator::initializeSensorData() { - // udp socket data - struct sockaddr_in _myaddr; - const int _port = UDP_PORT; + // write sensor data to memory so that drivers can copy data from there + RawMPUData mpu = {}; + mpu.accel_z = 9.81f; - struct baro_report baro; - memset(&baro,0,sizeof(baro)); - baro.pressure = 120000.0f; + write_MPU_data((void *)&mpu); - // acceleration report - struct accel_report accel; - memset(&accel,0,sizeof(accel)); + RawAccelData accel = {}; accel.z = 9.81f; - accel.range_m_s2 = 80.0f; - // gyro report - struct gyro_report gyro; - memset(&gyro, 0 ,sizeof(gyro)); + write_accel_data((void *)&accel); - // mag report - struct mag_report mag; - memset(&mag, 0 ,sizeof(mag)); + RawMagData mag = {}; + mag.x = 0.4f; + mag.y = 0.0f; + mag.z = 0.6f; - // init publishers - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + write_mag_data((void *)&mag); - // subscribe to topics - _actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); - _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + RawBaroData baro = {}; + // calculate air pressure from altitude (valid for low altitude) + baro.pressure = 120000.0f; + baro.altitude = 0.0f; + baro.temperature = 25.0f; + + write_baro_data((void *)&baro); + + RawAirspeedData airspeed {}; + + write_airspeed_data((void *)&airspeed); +} + +void Simulator::pollForMAVLinkMessages(bool publish) +{ + // udp socket data + struct sockaddr_in _myaddr; + const int _port = UDP_PORT; // try to setup udp socket for communcation with simulator memset((char *)&_myaddr, 0, sizeof(_myaddr)); @@ -328,41 +442,78 @@ void Simulator::updateSamples() /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT; (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); - pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); - pthread_attr_destroy(&sender_thread_attr); // setup serial connection to autopilot (used to get manual controls) - int serial_fd = open(PIXHAWK_DEVICE, O_RDWR); + int serial_fd = openUart(PIXHAWK_DEVICE, 115200); if (serial_fd < 0) { - PX4_WARN("failed to open %s", PIXHAWK_DEVICE); - } + PX4_INFO("Not using %s for radio control input. Assuming joystick input via MAVLink.", PIXHAWK_DEVICE); + + } else { - // tell the device to stream some messages - char command[] = "\nsh /etc/init.d/rc.usb\n"; - int w = ::write(serial_fd, command, sizeof(command)); + // tell the device to stream some messages + char command[] = "\nsh /etc/init.d/rc.usb\n"; + int w = ::write(serial_fd, command, sizeof(command)); - if (w <= 0) { - PX4_WARN("failed to send streaming command to %s", PIXHAWK_DEVICE); + if (w <= 0) { + PX4_WARN("failed to send streaming command to %s", PIXHAWK_DEVICE); + } } char serial_buf[1024]; struct pollfd fds[2]; + memset(fds, 0, sizeof(fds)); + unsigned fd_count = 1; fds[0].fd = _fd; fds[0].events = POLLIN; - fds[1].fd = serial_fd; - fds[1].events = POLLIN; + + + if (serial_fd >= 0) { + fds[1].fd = serial_fd; + fds[1].events = POLLIN; + fd_count++; + } int len = 0; + + // wait for first data from simulator and respond with first controls + // this is important for the UDP communication to work + int pret = -1; + PX4_INFO("Waiting for initial data on UDP. Please start the flight simulator to proceed.."); + + while (pret <= 0) { + pret = ::poll(&fds[0], (sizeof(fds[0]) / sizeof(fds[0])), 100); + } + + PX4_INFO("Found initial message, pret = %d", pret); + _initialized = true; + // reset system time + (void)hrt_reset(); + + if (fds[0].revents & POLLIN) { + len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + PX4_INFO("Sending initial controls message to jMAVSim."); + send_controls(); + } + + // subscribe to topics + _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + // got data from simulator, now activate the sending thread + pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); + pthread_attr_destroy(&sender_thread_attr); + // wait for new mavlink messages to arrive while (true) { - int pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100); + pret = ::poll(&fds[0], fd_count, 100); //timed out - if (pret == 0) + if (pret == 0) { continue; + } // this is undesirable but not much we can do if (pret < 0) { @@ -375,15 +526,15 @@ void Simulator::updateSamples() // got data from simulator if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + if (len > 0) { mavlink_message_t msg; mavlink_status_t status; - for (int i = 0; i < len; ++i) - { - if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) - { + + for (int i = 0; i < len; ++i) { + if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) { // have a message, handle it - handle_message(&msg); + handle_message(&msg, publish); } } } @@ -392,30 +543,265 @@ void Simulator::updateSamples() // got data from PIXHAWK if (fds[1].revents & POLLIN) { len = ::read(serial_fd, serial_buf, sizeof(serial_buf)); + if (len > 0) { mavlink_message_t msg; mavlink_status_t status; - for (int i = 0; i < len; ++i) - { - if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) - { + + for (int i = 0; i < len; ++i) { + if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) { // have a message, handle it - handle_message(&msg); + handle_message(&msg, true); } } } } + } +} + +int openUart(const char *uart_name, int baud) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 921600: speed = B921600; break; + + default: + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", + baud); + return -EINVAL; + } + + /* open uart */ + int uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + + if (uart_fd < 0) { + return uart_fd; + } + + + /* Try to set baud rate */ + struct termios uart_config; + memset(&uart_config, 0, sizeof(uart_config)); + + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) { + warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); + ::close(uart_fd); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart_fd, &uart_config); + + /* USB serial is indicated by /dev/ttyACM0*/ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); + ::close(uart_fd); + return -1; + } + + } + + // Make raw + cfmakeraw(&uart_config); + + if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR SET CONF %s\n", uart_name); + ::close(uart_fd); + return -1; + } + + return uart_fd; +} + +int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) +{ + + + //uint64_t timestamp = imu->time_usec; + uint64_t timestamp = hrt_absolute_time(); + + if ((imu->fields_updated & 0x1FFF) != 0x1FFF) { + PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated. Got %08x", imu->fields_updated); + } + + /* + static int count=0; + static uint64_t last_timestamp=0; + count++; + if (!(count % 200)) { + PX4_WARN("TIME : %lu, dt: %lu", + (unsigned long) timestamp,(unsigned long) timestamp - (unsigned long) last_timestamp); + PX4_WARN("IMU : %f %f %f",imu->xgyro,imu->ygyro,imu->zgyro); + PX4_WARN("ACCEL: %f %f %f",imu->xacc,imu->yacc,imu->zacc); + PX4_WARN("MAG : %f %f %f",imu->xmag,imu->ymag,imu->zmag); + PX4_WARN("BARO : %f %f %f",imu->abs_pressure,imu->pressure_alt,imu->temperature); + } + last_timestamp = timestamp; + */ + /* gyro */ + { + struct gyro_report gyro; + memset(&gyro, 0, sizeof(gyro)); + + gyro.timestamp = timestamp; + gyro.x_raw = imu->xgyro * 1000.0f; + gyro.y_raw = imu->ygyro * 1000.0f; + gyro.z_raw = imu->zgyro * 1000.0f; + gyro.x = imu->xgyro; + gyro.y = imu->ygyro; + gyro.z = imu->zgyro; + + if (_gyro_pub == nullptr) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } + + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); - // publish these messages so that attitude estimator does not complain - hrt_abstime time_last = hrt_absolute_time(); - baro.timestamp = time_last; - accel.timestamp = time_last; - gyro.timestamp = time_last; - mag.timestamp = time_last; - // publish the sensor values - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + accel.timestamp = timestamp; + accel.x_raw = imu->xacc / mg2ms2; + accel.y_raw = imu->yacc / mg2ms2; + accel.z_raw = imu->zacc / mg2ms2; + accel.x = imu->xacc; + accel.y = imu->yacc; + accel.z = imu->zacc; + + if (_accel_pub == nullptr) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } } + + /* magnetometer */ + { + struct mag_report mag; + memset(&mag, 0, sizeof(mag)); + + mag.timestamp = timestamp; + mag.x_raw = imu->xmag * 1000.0f; + mag.y_raw = imu->ymag * 1000.0f; + mag.z_raw = imu->zmag * 1000.0f; + mag.x = imu->xmag; + mag.y = imu->ymag; + mag.z = imu->zmag; + + if (_mag_pub == nullptr) { + /* publish to the first mag topic */ + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + + } else { + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + } + + /* baro */ + { + struct baro_report baro; + memset(&baro, 0, sizeof(baro)); + + baro.timestamp = timestamp; + baro.pressure = imu->abs_pressure; + baro.altitude = imu->pressure_alt; + baro.temperature = imu->temperature; + + if (_baro_pub == nullptr) { + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + + } else { + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + } + } + + return OK; +} + +int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink) +{ + uint64_t timestamp = hrt_absolute_time(); + + /* flow */ + { + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); + + flow.sensor_id = flow_mavlink->sensor_id; + flow.timestamp = timestamp; + flow.time_since_last_sonar_update = 0; + flow.frame_count_since_last_readout = 0; // ? + flow.integration_timespan = flow_mavlink->integration_time_us; + + flow.ground_distance_m = flow_mavlink->distance; + flow.gyro_temperature = flow_mavlink->temperature; + flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro; + flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro; + flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro; + flow.pixel_flow_x_integral = flow_mavlink->integrated_x; + flow.pixel_flow_x_integral = flow_mavlink->integrated_y; + flow.quality = flow_mavlink->quality; + + if (_flow_pub == nullptr) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &flow); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &flow); + } + } + + return OK; } diff --git a/src/modules/systemlib/mixer/mixer_multirotor.generated.h b/src/modules/systemlib/mixer/mixer_multirotor.generated.h new file mode 100644 index 000000000000..98197bdbde25 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_multirotor.generated.h @@ -0,0 +1,168 @@ +/* +* This file is automatically generated by multi_tables - do not edit. +*/ + +#ifndef _MIXER_MULTI_TABLES +#define _MIXER_MULTI_TABLES + +enum class MultirotorGeometry : MultirotorGeometryUnderlyingType { + QUAD_X, + QUAD_PLUS, + QUAD_V, + QUAD_WIDE, + QUAD_DEADCAT, + HEX_X, + HEX_PLUS, + HEX_COX, + OCTA_X, + OCTA_PLUS, + OCTA_COX, + TWIN_ENGINE, + TRI_Y, + + MAX_GEOMETRY +}; // enum class MultirotorGeometry + +namespace { +const MultirotorMixer::Rotor _config_quad_x[] = { + { -0.707107, 0.707107, 1.000000, 1.000000 }, + { 0.707107, -0.707107, 1.000000, 1.000000 }, + { 0.707107, 0.707107, -1.000000, 1.000000 }, + { -0.707107, -0.707107, -1.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor _config_quad_plus[] = { + { -1.000000, 0.000000, 1.000000, 1.000000 }, + { 1.000000, 0.000000, 1.000000, 1.000000 }, + { 0.000000, 1.000000, -1.000000, 1.000000 }, + { -0.000000, -1.000000, -1.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor _config_quad_v[] = { + { -0.322266, 0.946649, 0.424200, 1.000000 }, + { 0.322266, 0.946649, 1.000000, 1.000000 }, + { 0.322266, 0.946649, -0.424200, 1.000000 }, + { -0.322266, 0.946649, -1.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor _config_quad_wide[] = { + { -0.927184, 0.374607, 1.000000, 1.000000 }, + { 0.777146, -0.629320, 1.000000, 1.000000 }, + { 0.927184, 0.374607, -1.000000, 1.000000 }, + { -0.777146, -0.629320, -1.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor _config_quad_deadcat[] = { + { -0.891007, 0.453990, 1.000000, 1.000000 }, + { 0.707107, -0.707107, 1.000000, 0.964000 }, + { 0.891007, 0.453990, -1.000000, 1.000000 }, + { -0.707107, -0.707107, -1.000000, 0.964000 }, +}; + +const MultirotorMixer::Rotor _config_hex_x[] = { + { -1.000000, 0.000000, -1.000000, 1.000000 }, + { 1.000000, 0.000000, 1.000000, 1.000000 }, + { 0.500000, 0.866025, -1.000000, 1.000000 }, + { -0.500000, -0.866025, 1.000000, 1.000000 }, + { -0.500000, 0.866025, 1.000000, 1.000000 }, + { 0.500000, -0.866025, -1.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor _config_hex_plus[] = { + { 0.000000, 1.000000, -1.000000, 1.000000 }, + { -0.000000, -1.000000, 1.000000, 1.000000 }, + { 0.866025, -0.500000, -1.000000, 1.000000 }, + { -0.866025, 0.500000, 1.000000, 1.000000 }, + { 0.866025, 0.500000, 1.000000, 1.000000 }, + { -0.866025, -0.500000, -1.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor _config_hex_cox[] = { + { -0.866025, 0.500000, -1.000000, 1.000000 }, + { -0.866025, 0.500000, 1.000000, 1.000000 }, + { -0.000000, -1.000000, -1.000000, 1.000000 }, + { -0.000000, -1.000000, 1.000000, 1.000000 }, + { 0.866025, 0.500000, -1.000000, 1.000000 }, + { 0.866025, 0.500000, 1.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor _config_octa_x[] = { + { -0.382683, 0.923880, -1.000000, 1.000000 }, + { 0.382683, -0.923880, -1.000000, 1.000000 }, + { -0.923880, 0.382683, 1.000000, 1.000000 }, + { -0.382683, -0.923880, 1.000000, 1.000000 }, + { 0.382683, 0.923880, 1.000000, 1.000000 }, + { 0.923880, -0.382683, 1.000000, 1.000000 }, + { 0.923880, 0.382683, -1.000000, 1.000000 }, + { -0.923880, -0.382683, -1.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor _config_octa_plus[] = { + { 0.000000, 1.000000, -1.000000, 1.000000 }, + { -0.000000, -1.000000, -1.000000, 1.000000 }, + { -0.707107, 0.707107, 1.000000, 1.000000 }, + { -0.707107, -0.707107, 1.000000, 1.000000 }, + { 0.707107, 0.707107, 1.000000, 1.000000 }, + { 0.707107, -0.707107, 1.000000, 1.000000 }, + { 1.000000, 0.000000, -1.000000, 1.000000 }, + { -1.000000, 0.000000, -1.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor _config_octa_cox[] = { + { -0.707107, 0.707107, 1.000000, 1.000000 }, + { 0.707107, 0.707107, -1.000000, 1.000000 }, + { 0.707107, -0.707107, 1.000000, 1.000000 }, + { -0.707107, -0.707107, -1.000000, 1.000000 }, + { 0.707107, 0.707107, 1.000000, 1.000000 }, + { -0.707107, 0.707107, -1.000000, 1.000000 }, + { -0.707107, -0.707107, 1.000000, 1.000000 }, + { 0.707107, -0.707107, -1.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor _config_twin_engine[] = { + { -1.000000, 0.000000, 0.000000, 1.000000 }, + { 1.000000, 0.000000, 0.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor _config_tri_y[] = { + { -0.866025, 0.500000, 0.000000, 1.000000 }, + { 0.866025, 0.500000, 0.000000, 1.000000 }, + { -0.000000, -1.000000, 0.000000, 1.000000 }, +}; + +const MultirotorMixer::Rotor *_config_index[] = { + &_config_quad_x[0], + &_config_quad_plus[0], + &_config_quad_v[0], + &_config_quad_wide[0], + &_config_quad_deadcat[0], + &_config_hex_x[0], + &_config_hex_plus[0], + &_config_hex_cox[0], + &_config_octa_x[0], + &_config_octa_plus[0], + &_config_octa_cox[0], + &_config_twin_engine[0], + &_config_tri_y[0], +}; + +const unsigned _config_rotor_count[] = { + 4, /* quad_x */ + 4, /* quad_plus */ + 4, /* quad_v */ + 4, /* quad_wide */ + 4, /* quad_deadcat */ + 6, /* hex_x */ + 6, /* hex_plus */ + 6, /* hex_cox */ + 8, /* octa_x */ + 8, /* octa_plus */ + 8, /* octa_cox */ + 2, /* twin_engine */ + 3, /* tri_y */ +}; + +} // anonymous namespace + +#endif /* _MIXER_MULTI_TABLES */ + diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt new file mode 100644 index 000000000000..840b1307ae89 --- /dev/null +++ b/src/modules/uORB/CMakeLists.txt @@ -0,0 +1,83 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# this includes the generated topics directory +include_directories( + ${CMAKE_CURRENT_BINARY_DIR} + ) + +set(SRCS + objects_common.cpp + uORBUtils.cpp + uORB.cpp + uORBMain.cpp + Publication.cpp + Subscription.cpp + ) + +if(${OS} STREQUAL "nuttx") + list(APPEND SRCS + uORBDevices_nuttx.cpp + uORBTest_UnitTest.cpp + uORBManager_nuttx.cpp + ) +elseif(${OS} STREQUAL "posix") + list(APPEND SRCS + uORBDevices_posix.cpp + uORBManager_posix.cpp + uORBTest_UnitTest.cpp + ) +elseif(${OS} STREQUAL "posix-arm") + list(APPEND SRCS + uORBDevices_posix.cpp + uORBManager_posix.cpp + uORBTest_UnitTest.cpp + ) +elseif(${OS} STREQUAL "qurt") + list(APPEND SRCS + uORBDevices_posix.cpp + uORBManager_posix.cpp + ) +endif() + +px4_add_module( + MODULE modules__uORB + MAIN uorb + STACK 2048 + COMPILE_FLAGS + -Os + SRCS ${SRCS} + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/uORB/ORBMap.hpp b/src/modules/uORB/ORBMap.hpp new file mode 100644 index 000000000000..3d2673580914 --- /dev/null +++ b/src/modules/uORB/ORBMap.hpp @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. 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 + +#include +#include + + +namespace uORB +{ +class DeviceNode; +class ORBMap; +} + +class uORB::ORBMap +{ +public: + struct Node { + struct Node *next; + const char *node_name; + uORB::DeviceNode *node; + }; + + ORBMap() : + _top(nullptr), + _end(nullptr) + { } + ~ORBMap() + { + while (_top != nullptr) { + unlinkNext(_top); + + if (_top->next == nullptr) { + free((void *)_top->node_name); + free(_top); + _top = nullptr; + _end = nullptr; + } + } + } + void insert(const char *node_name, uORB::DeviceNode *node) + { + Node **p; + + if (_top == nullptr) { + p = &_top; + + } else { + p = &_end->next; + } + + *p = (Node *)malloc(sizeof(Node)); + + if (_end) { + _end = _end->next; + + } else { + _end = _top; + } + + _end->next = nullptr; + _end->node_name = strdup(node_name); + _end->node = node; + } + + bool find(const char *node_name) + { + Node *p = _top; + + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return true; + } + + p = p->next; + } + + return false; + } + + uORB::DeviceNode *get(const char *node_name) + { + Node *p = _top; + + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return p->node; + } + + p = p->next; + } + + return nullptr; + } + + void unlinkNext(Node *a) + { + Node *b = a->next; + + if (b != nullptr) { + if (_end == b) { + _end = a; + } + + a->next = b->next; + free((void *)b->node_name); + free(b); + } + } + +private: + Node *_top; + Node *_end; +}; + diff --git a/src/modules/uORB/ORBSet.hpp b/src/modules/uORB/ORBSet.hpp new file mode 100644 index 000000000000..94ba85138eb4 --- /dev/null +++ b/src/modules/uORB/ORBSet.hpp @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. 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 + +class ORBSet +{ +public: + struct Node { + struct Node *next; + const char *node_name; + }; + + ORBSet() : + _top(nullptr), + _end(nullptr) + { } + ~ORBSet() + { + while (_top != nullptr) { + unlinkNext(_top); + + if (_top->next == nullptr) { + free((void *)_top->node_name); + free(_top); + _top = nullptr; + } + } + } + void insert(const char *node_name) + { + Node **p; + + if (_top == nullptr) { + p = &_top; + + } else { + p = &_end->next; + } + + *p = (Node *)malloc(sizeof(Node)); + + if (_end) { + _end = _end->next; + + } else { + _end = _top; + } + + _end->next = nullptr; + _end->node_name = strdup(node_name); + } + + bool find(const char *node_name) + { + Node *p = _top; + + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return true; + } + + p = p->next; + } + + return false; + } + + bool erase(const char *node_name) + { + Node *p = _top; + + if (_top && (strcmp(_top->node_name, node_name) == 0)) { + p = _top->next; + free((void *)_top->node_name); + free(_top); + _top = p; + + if (_top == nullptr) { + _end = nullptr; + } + + return true; + } + + while (p->next) { + if (strcmp(p->next->node_name, node_name) == 0) { + unlinkNext(p); + return true; + } + } + + return false; + } + +private: + + void unlinkNext(Node *a) + { + Node *b = a->next; + + if (b != nullptr) { + if (_end == b) { + _end = a; + } + + a->next = b->next; + free((void *)b->node_name); + free(b); + } + } + + Node *_top; + Node *_end; +}; + diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 0ea8e5db51ff..e979e3b4f484 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -50,33 +50,63 @@ #include "topics/encoders.h" #include "topics/tecs_status.h" #include "topics/rc_channels.h" +#include "topics/filtered_bottom_flow.h" -namespace uORB { +#include -template -Publication::Publication( - const struct orb_metadata *meta, - List * list) : - T(), // initialize data structure to zero - PublicationNode(meta, list) { +namespace uORB +{ + +PublicationBase::PublicationBase(const struct orb_metadata *meta, + int priority) : + _meta(meta), + _priority(priority), + _instance(), + _handle(nullptr) +{ } -template -Publication::~Publication() {} +void PublicationBase::update(void *data) +{ + if (_handle != nullptr) { + int ret = orb_publish(getMeta(), getHandle(), data); + + if (ret != PX4_OK) { warnx("publish fail"); } + + } else { + orb_advert_t handle; + + if (_priority > 0) { + handle = orb_advertise_multi( + getMeta(), data, + &_instance, _priority); -template -void * Publication::getDataVoidPtr() { - return (void *)(T *)(this); + } else { + handle = orb_advertise(getMeta(), data); + } + + if (int64_t(handle) != PX4_ERROR) { + setHandle(handle); + + } else { + warnx("advert fail"); + } + } } +PublicationBase::~PublicationBase() +{ +} PublicationNode::PublicationNode(const struct orb_metadata *meta, - List * list) : - PublicationBase(meta) { - if (list != nullptr) list->add(this); + int priority, + List *list) : + PublicationBase(meta, priority) +{ + if (list != nullptr) { list->add(this); } } - +// explicit template instantiation template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; @@ -90,5 +120,6 @@ 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/Publication.hpp b/src/modules/uORB/Publication.hpp index 6a0c733c640c..37658721ca29 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -42,13 +42,13 @@ #include #include - +#include namespace uORB { /** - * Base publication warapper class, used in list traversal + * Base publication wrapper class, used in list traversal * of various publications. */ class __EXPORT PublicationBase @@ -58,46 +58,40 @@ class __EXPORT PublicationBase /** * Constructor * - * - * @param meta The uORB metadata (usually from the ORB_ID() - * macro) for the topic. + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub/sub, 0-based, -1 means + * don't publish as multi */ - PublicationBase(const struct orb_metadata *meta) : - _meta(meta), - _handle(nullptr) { - } + PublicationBase(const struct orb_metadata *meta, + int priority = -1); /** * Update the struct * @param data The uORB message struct we are updating. */ - void update(void * data) { - if (_handle != nullptr) { - orb_publish(getMeta(), getHandle(), data); - } else { - setHandle(orb_advertise(getMeta(), data)); - } - } + void update(void *data); /** * Deconstructor */ - virtual ~PublicationBase() { - } + virtual ~PublicationBase(); + // accessors const struct orb_metadata *getMeta() { return _meta; } orb_advert_t getHandle() { return _handle; } protected: + // disallow copy + PublicationBase(const PublicationBase &other); + // disallow assignment + PublicationBase &operator=(const PublicationBase &other); // accessors void setHandle(orb_advert_t handle) { _handle = handle; } // attributes const struct orb_metadata *_meta; + int _priority; + int _instance; orb_advert_t _handle; -private: - // forbid copy - PublicationBase(const PublicationBase&) : _meta(), _handle() {}; - // forbid assignment - PublicationBase& operator = (const PublicationBase &); }; /** @@ -117,14 +111,15 @@ class __EXPORT PublicationNode : /** * Constructor * - * - * @param meta The uORB metadata (usually from the ORB_ID() - * macro) for the topic. - * @param list A pointer to a list of subscriptions - * that this should be appended to. + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub, 0-based. + * @param list A list interface for adding to + * list during construction */ PublicationNode(const struct orb_metadata *meta, - List * list=nullptr); + int priority = -1, + List *list = nullptr); /** * This function is the callback for list traversal @@ -138,7 +133,6 @@ class __EXPORT PublicationNode : */ template class __EXPORT Publication : - public T, // this must be first! public PublicationNode { public: @@ -147,32 +141,37 @@ class __EXPORT Publication : * * @param meta The uORB metadata (usually from * the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub, 0-based. * @param list A list interface for adding to * list during construction */ Publication(const struct orb_metadata *meta, - List * list=nullptr); + int priority = -1, + List *list = nullptr) : + PublicationNode(meta, priority, list), + _data() + { + } /** * Deconstructor **/ - virtual ~Publication(); + virtual ~Publication() {}; /* - * XXX - * This function gets the T struct, assuming - * the struct is the first base class, this - * should use dynamic cast, but doesn't - * seem to be available - */ - void *getDataVoidPtr(); + * This function gets the T struct + * */ + T &get() { return _data; } /** * Create an update function that uses the embedded struct. */ - void update() { - PublicationBase::update(getDataVoidPtr()); + void update() + { + PublicationBase::update((void *)(&_data)); } +private: + T _data; }; } // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 0c9433f0366b..a85e8da02dec 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -42,6 +42,7 @@ #include "topics/vehicle_gps_position.h" #include "topics/satellite_info.h" #include "topics/sensor_combined.h" +#include "topics/hil_sensor.h" #include "topics/vehicle_attitude.h" #include "topics/vehicle_global_position.h" #include "topics/encoders.h" @@ -53,39 +54,95 @@ #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" #include "topics/rc_channels.h" +#include "topics/battery_status.h" +#include "topics/optical_flow.h" +#include "topics/distance_sensor.h" +#include "topics/home_position.h" #include "topics/vehicle_control_mode.h" #include "topics/actuator_armed.h" +#include "topics/att_pos_mocap.h" +#include "topics/vision_position_estimate.h" + +#include namespace uORB { -template -Subscription::Subscription( - const struct orb_metadata *meta, - unsigned interval, - List * list) : - T(), // initialize data structure to zero - SubscriptionNode(meta, interval, list) { +SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta, + unsigned interval, unsigned instance) : + _meta(meta), + _instance(instance), + _handle() +{ + if (_instance > 0) { + _handle = orb_subscribe_multi( + getMeta(), instance); + + } else { + _handle = orb_subscribe(getMeta()); + } + + if (_handle < 0) { warnx("sub failed"); } + + orb_set_interval(getHandle(), interval); } -template -Subscription::~Subscription() {} +bool SubscriptionBase::updated() +{ + bool isUpdated = false; + int ret = orb_check(_handle, &isUpdated); + + if (ret != PX4_OK) { warnx("orb check failed"); } -template -void * Subscription::getDataVoidPtr() { - return (void *)(T *)(this); + return isUpdated; } -template -T Subscription::getData() { - return T(*this); +void SubscriptionBase::update(void *data) +{ + if (updated()) { + int ret = orb_copy(_meta, _handle, data); + + if (ret != PX4_OK) { warnx("orb copy failed"); } + } } +SubscriptionBase::~SubscriptionBase() +{ + int ret = orb_unsubscribe(_handle); + + if (ret != PX4_OK) { warnx("orb unsubscribe failed"); } +} + +template +Subscription::Subscription(const struct orb_metadata *meta, + unsigned interval, + int instance, + List *list) : + SubscriptionNode(meta, interval, instance, list), + _data() // initialize data structure to zero +{ +} + +template +Subscription::~Subscription() +{ +} + +template +void Subscription::update() +{ + SubscriptionBase::update((void *)(&_data)); +} + +template +const T &Subscription::get() { return _data; } + template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; @@ -99,5 +156,11 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 31842ed715ae..0136cfcacc13 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -42,6 +42,7 @@ #include #include +#include namespace uORB { @@ -60,43 +61,29 @@ class __EXPORT SubscriptionBase * * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. - * * @param interval The minimum interval in milliseconds * between updates + * @param instance The instance for multi sub. */ SubscriptionBase(const struct orb_metadata *meta, - unsigned interval=0) : - _meta(meta), - _handle() { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); - } + unsigned interval = 0, unsigned instance = 0); /** * Check if there is a new update. * */ - bool updated() { - bool isUpdated = false; - orb_check(_handle, &isUpdated); - return isUpdated; - } + bool updated(); /** * Update the struct * @param data The uORB message struct we are updating. */ - void update(void * data) { - if (updated()) { - orb_copy(_meta, _handle, data); - } - } + void update(void *data); /** * Deconstructor */ - virtual ~SubscriptionBase() { - orb_unsubscribe(_handle); - } + virtual ~SubscriptionBase(); + // accessors const struct orb_metadata *getMeta() { return _meta; } int getHandle() { return _handle; } @@ -105,12 +92,13 @@ class __EXPORT SubscriptionBase void setHandle(int handle) { _handle = handle; } // attributes const struct orb_metadata *_meta; + int _instance; int _handle; private: - // forbid copy - SubscriptionBase(const SubscriptionBase& other); - // forbid assignment - SubscriptionBase& operator = (const SubscriptionBase &); + // disallow copy + SubscriptionBase(const SubscriptionBase &other); + // disallow assignment + SubscriptionBase &operator=(const SubscriptionBase &other); }; /** @@ -119,7 +107,7 @@ class __EXPORT SubscriptionBase typedef SubscriptionBase SubscriptionTiny; /** - * The publication base class as a list node. + * The subscription base class as a list node. */ class __EXPORT SubscriptionNode : @@ -130,20 +118,22 @@ class __EXPORT SubscriptionNode : /** * Constructor * - * * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. * @param interval The minimum interval in milliseconds * between updates + * @param instance The instance for multi sub. * @param list A pointer to a list of subscriptions * that this should be appended to. */ SubscriptionNode(const struct orb_metadata *meta, - unsigned interval=0, - List * list=nullptr) : - SubscriptionBase(meta, interval), - _interval(interval) { - if (list != nullptr) list->add(this); + unsigned interval = 0, + int instance = 0, + List *list = nullptr) : + SubscriptionBase(meta, interval, instance), + _interval(interval) + { + if (list != nullptr) { list->add(this); } } /** @@ -164,7 +154,6 @@ class __EXPORT SubscriptionNode : */ template class __EXPORT Subscription : - public T, // this must be first! public SubscriptionNode { public: @@ -179,8 +168,10 @@ class __EXPORT Subscription : * list during construction */ Subscription(const struct orb_metadata *meta, - unsigned interval=0, - List * list=nullptr); + unsigned interval = 0, + int instance = 0, + List *list = nullptr); + /** * Deconstructor */ @@ -190,19 +181,14 @@ class __EXPORT Subscription : /** * Create an update function that uses the embedded struct. */ - void update() { - SubscriptionBase::update(getDataVoidPtr()); - } + void update(); /* - * XXX - * This function gets the T struct, assuming - * the struct is the first base class, this - * should use dynamic cast, but doesn't - * seem to be available - */ - void *getDataVoidPtr(); - T getData(); + * This function gets the T struct data + * */ + const T &get(); +private: + T _data; }; } // namespace uORB diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 9f980eebeae5..8fe70d7604f6 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -45,23 +45,23 @@ #include -#include -ORB_DEFINE(sensor_mag, struct mag_report); +#include "topics/sensor_mag.h" +ORB_DEFINE(sensor_mag, struct sensor_mag_s); -#include -ORB_DEFINE(sensor_accel, struct accel_report); +#include "topics/sensor_accel.h" +ORB_DEFINE(sensor_accel, struct sensor_accel_s); -#include -ORB_DEFINE(sensor_gyro, struct gyro_report); +#include "topics/sensor_gyro.h" +ORB_DEFINE(sensor_gyro, struct sensor_gyro_s); -#include -ORB_DEFINE(sensor_baro, struct baro_report); +#include "topics/sensor_baro.h" +ORB_DEFINE(sensor_baro, struct sensor_baro_s); -#include -ORB_DEFINE(output_pwm, struct pwm_output_values); +#include "topics/output_pwm.h" +ORB_DEFINE(output_pwm, struct output_pwm_s); -#include -ORB_DEFINE(input_rc, struct rc_input_values); +#include "topics/input_rc.h" +ORB_DEFINE(input_rc, struct input_rc_s); #include "topics/pwm_input.h" ORB_DEFINE(pwm_input, struct pwm_input_s); @@ -69,9 +69,15 @@ ORB_DEFINE(pwm_input, struct pwm_input_s); #include "topics/vehicle_attitude.h" ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); +#include "topics/control_state.h" +ORB_DEFINE(control_state, struct control_state_s); + #include "topics/sensor_combined.h" ORB_DEFINE(sensor_combined, struct sensor_combined_s); +#include "topics/hil_sensor.h" +ORB_DEFINE(hil_sensor, struct hil_sensor_s); + #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); @@ -108,8 +114,8 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); #include "topics/vehicle_local_position.h" ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s); -#include "topics/vehicle_vicon_position.h" -ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s); +#include "topics/att_pos_mocap.h" +ORB_DEFINE(att_pos_mocap, struct att_pos_mocap_s); #include "topics/vehicle_rates_setpoint.h" ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); @@ -137,6 +143,10 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" +ORB_DEFINE(mission, struct mission_s); +// XXX onboard and offboard mission are still declared here until this is +// generator supported +#include ORB_DEFINE(offboard_mission, struct mission_s); ORB_DEFINE(onboard_mission, struct mission_s); @@ -207,10 +217,7 @@ ORB_DEFINE(actuator_direct, struct actuator_direct_s); ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s); #include "topics/telemetry_status.h" -ORB_DEFINE(telemetry_status_0, struct telemetry_status_s); -ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); -ORB_DEFINE(telemetry_status_2, struct telemetry_status_s); -ORB_DEFINE(telemetry_status_3, struct telemetry_status_s); +ORB_DEFINE(telemetry_status, struct telemetry_status_s); #include "topics/test_motor.h" ORB_DEFINE(test_motor, struct test_motor_s); @@ -256,3 +263,6 @@ ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s); #include "topics/distance_sensor.h" ORB_DEFINE(distance_sensor, struct distance_sensor_s); + +#include "topics/camera_trigger.h" +ORB_DEFINE(camera_trigger, struct camera_trigger_s); diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index c1266552fe11..6d6f084d2469 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -62,7 +62,7 @@ */ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { - return uORB::Manager::get_instance()->orb_advertise( meta, data ); + return uORB::Manager::get_instance()->orb_advertise(meta, data); } /** @@ -91,9 +91,9 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) * this function will return -1 and set errno to ENOENT. */ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - int priority) + int priority) { - return uORB::Manager::get_instance()->orb_advertise_multi( meta, data, instance, priority ); + return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority); } @@ -112,7 +112,7 @@ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *da */ int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { - return uORB::Manager::get_instance()->orb_publish( meta, handle, data ); + return uORB::Manager::get_instance()->orb_publish(meta, handle, data); } /** @@ -143,7 +143,7 @@ int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const voi */ int orb_subscribe(const struct orb_metadata *meta) { - return uORB::Manager::get_instance()->orb_subscribe( meta ); + return uORB::Manager::get_instance()->orb_subscribe(meta); } /** @@ -177,7 +177,7 @@ int orb_subscribe(const struct orb_metadata *meta) */ int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) { - return uORB::Manager::get_instance()->orb_subscribe_multi( meta, instance ); + return uORB::Manager::get_instance()->orb_subscribe_multi(meta, instance); } /** @@ -188,7 +188,7 @@ int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) */ int orb_unsubscribe(int handle) { - return uORB::Manager::get_instance()->orb_unsubscribe( handle ); + return uORB::Manager::get_instance()->orb_unsubscribe(handle); } /** @@ -209,7 +209,7 @@ int orb_unsubscribe(int handle) */ int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) { - return uORB::Manager::get_instance()->orb_copy( meta, handle, buffer ); + return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer); } /** @@ -232,7 +232,7 @@ int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) */ int orb_check(int handle, bool *updated) { - return uORB::Manager::get_instance()->orb_check( handle, updated ); + return uORB::Manager::get_instance()->orb_check(handle, updated); } /** @@ -245,7 +245,7 @@ int orb_check(int handle, bool *updated) */ int orb_stat(int handle, uint64_t *time) { - return uORB::Manager::get_instance()->orb_stat( handle, time ); + return uORB::Manager::get_instance()->orb_stat(handle, time); } /** @@ -257,7 +257,22 @@ int orb_stat(int handle, uint64_t *time) */ int orb_exists(const struct orb_metadata *meta, int instance) { - return uORB::Manager::get_instance()->orb_exists( meta, instance ); + return uORB::Manager::get_instance()->orb_exists(meta, instance); +} + +/** + * Get the number of published instances of a topic group + * + * @param meta ORB topic metadata. + * @return The number of published instances of this topic + */ +int orb_group_count(const struct orb_metadata *meta) +{ + unsigned group_count = 0; + + while (!uORB::Manager::get_instance()->orb_exists(meta, group_count++)) {}; + + return group_count; } /** @@ -270,9 +285,9 @@ int orb_exists(const struct orb_metadata *meta, int instance) * priority, independent of the startup order of the associated publishers. * @return OK on success, ERROR otherwise with errno set accordingly. */ -int orb_priority(int handle, int *priority) +int orb_priority(int handle, int32_t *priority) { - return uORB::Manager::get_instance()->orb_priority( handle, priority ); + return uORB::Manager::get_instance()->orb_priority(handle, priority); } /** @@ -295,6 +310,6 @@ int orb_priority(int handle, int *priority) */ int orb_set_interval(int handle, unsigned interval) { - return uORB::Manager::get_instance()->orb_set_interval( handle, interval ); + return uORB::Manager::get_instance()->orb_set_interval(handle, interval); } diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index e9c7954bf534..6ead26186ba1 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -59,7 +59,7 @@ typedef const struct orb_metadata *orb_id_t; /** * Maximum number of multi topic instances */ -#define ORB_MULTI_MAX_INSTANCES 3 +#define ORB_MULTI_MAX_INSTANCES 4 /** * Topic priority. @@ -89,20 +89,14 @@ enum ORB_PRIO { /** * Declare (prototype) the uORB metadata for a topic. * - * Note that optional topics are declared weak; this allows a potential - * subscriber to attempt to subscribe to a topic that is not known to the - * system at runtime. The ORB_ID() macro will return NULL/nullptr for - * such a topic, and attempts to advertise or subscribe to it will - * return -1/ENOENT (see below). - * * @param _name The name of the topic. */ #if defined(__cplusplus) # define ORB_DECLARE(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT -# define ORB_DECLARE_OPTIONAL(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT __attribute__((weak)) +# define ORB_DECLARE_OPTIONAL(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT #else # define ORB_DECLARE(_name) extern const struct orb_metadata __orb_##_name __EXPORT -# define ORB_DECLARE_OPTIONAL(_name) extern const struct orb_metadata __orb_##_name __EXPORT __attribute__((weak)) +# define ORB_DECLARE_OPTIONAL(_name) extern const struct orb_metadata __orb_##_name __EXPORT #endif /** @@ -135,7 +129,7 @@ __BEGIN_DECLS * a file-descriptor-based handle would not otherwise be in scope for the * publisher. */ -typedef void * orb_advert_t; +typedef void *orb_advert_t; /** * Advertise as the publisher of a topic. @@ -173,8 +167,8 @@ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *d * @param data A pointer to the initial data to be published. * For topics updated by interrupt handlers, the advertisement * must be performed from non-interrupt context. - * @param instance Pointer to an integer which will yield the instance ID (0-based) - * of the publication. + * @param instance Pointer to an integer which will yield the instance ID (0-based, + * limited by ORB_MULTI_MAX_INSTANCES) of the publication. * @param priority The priority of the instance. If a subscriber subscribes multiple * instances, the priority allows the subscriber to prioritize the best * data source as long as its available. @@ -254,6 +248,7 @@ extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; * @param instance The instance of the topic. Instance 0 matches the * topic of the orb_subscribe() call, higher indices * are for topics created with orb_publish_multi(). + * Instance is limited by ORB_MULTI_MAX_INSTANCES. * @return ERROR on error, otherwise returns a handle * that can be used to read and update the topic. * If the topic in question is not known (due to an @@ -327,6 +322,14 @@ extern int orb_stat(int handle, uint64_t *time) __EXPORT; */ extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT; +/** + * Get the number of published instances of a topic group + * + * @param meta ORB topic metadata. + * @return The number of published instances of this topic + */ +extern int orb_group_count(const struct orb_metadata *meta) __EXPORT; + /** * Return the priority of the topic * @@ -337,7 +340,7 @@ extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT; * priority, independent of the startup order of the associated publishers. * @return OK on success, ERROR otherwise with errno set accordingly. */ -extern int orb_priority(int handle, int *priority) __EXPORT; +extern int orb_priority(int handle, int32_t *priority) __EXPORT; /** * Set the minimum interval between which updates are seen for a subscription. diff --git a/src/modules/uORB/uORBCommon.hpp b/src/modules/uORB/uORBCommon.hpp index 06f731e8235d..be2ad8a89d05 100644 --- a/src/modules/uORB/uORBCommon.hpp +++ b/src/modules/uORB/uORBCommon.hpp @@ -43,23 +43,23 @@ namespace uORB { - static const unsigned orb_maxpath = 64; +static const unsigned orb_maxpath = 64; - #ifdef ERROR - # undef ERROR - #endif - /* ERROR is not defined for c++ */ - const int ERROR = -1; +#ifdef ERROR +# undef ERROR +#endif +/* ERROR is not defined for c++ */ +const int ERROR = -1; - enum Flavor { - PUBSUB, - PARAM - }; +enum Flavor { + PUBSUB, + PARAM +}; - struct orb_advertdata { - const struct orb_metadata *meta; - int *instance; - int priority; - }; +struct orb_advertdata { + const struct orb_metadata *meta; + int *instance; + int priority; +}; } #endif // _uORBCommon_hpp_ diff --git a/src/modules/uORB/uORBCommunicator.hpp b/src/modules/uORB/uORBCommunicator.hpp new file mode 100644 index 000000000000..971ee3ab0133 --- /dev/null +++ b/src/modules/uORB/uORBCommunicator.hpp @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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. + * + ****************************************************************************/ + +#ifndef _uORBCommunicator_hpp_ +#define _uORBCommunicator_hpp_ + +#include + + +namespace uORBCommunicator +{ +class IChannel; +class IChannelRxHandler; +} + +/** + * Interface to enable remote subscriptions. The implementor of this interface + * shall manage the communication channel. It can be fastRPC or tcp or ip. + */ + +class uORBCommunicator::IChannel +{ +public: + + //========================================================================= + // INTERFACES FOR Control messages over a channel. + //========================================================================= + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz) = 0; + + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + + virtual int16_t remove_subscription(const char *messageName) = 0; + + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler) = 0; + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data) = 0; + +}; + +/** + * Class passed to the communication link implement to provide callback for received + * messages over a channel. + */ +class uORBCommunicator::IChannelRxHandler +{ +public: + + /** + * Interface to process a received AddSubscription from remote. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + + virtual int16_t process_add_subscription(const char *messageName, int32_t msgRateInHz) = 0; + + + /** + * Interface to process a received control msg to remove subscription + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + + virtual int16_t process_remove_subscription(const char *messageName) = 0; + + + /** + * Interface to process the received data message. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + + virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data) = 0; + +}; + +#endif /* _uORBCommunicator_hpp_ */ diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index d3c30f05085d..55f9d353f3f0 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -38,396 +38,518 @@ #include #include "uORBDevices_nuttx.hpp" #include "uORBUtils.hpp" +#include "uORBManager.hpp" +#include "uORBCommunicator.hpp" #include +uORB::ORBMap uORB::DeviceMaster::_node_map; + uORB::DeviceNode::DeviceNode ( - const struct orb_metadata *meta, - const char *name, - const char *path, - int priority - ) : - CDev(name, path), - _meta(meta), - _data(nullptr), - _last_update(0), - _generation(0), - _publisher(0), - _priority(priority) + const struct orb_metadata *meta, + const char *name, + const char *path, + int priority +) : + CDev(name, path), + _meta(meta), + _data(nullptr), + _last_update(0), + _generation(0), + _publisher(0), + _priority(priority), + _published(false), + _IsRemoteSubscriberPresent(false), + _subscriber_count(0) { - // enable debug() calls - _debug_enabled = true; + // enable debug() calls + _debug_enabled = true; } uORB::DeviceNode::~DeviceNode() { - if (_data != nullptr) - delete[] _data; + if (_data != nullptr) { + delete[] _data; + } } int uORB::DeviceNode::open(struct file *filp) { - int ret; + int ret; + + /* is this a publisher? */ + if (filp->f_oflags == O_WRONLY) { - /* is this a publisher? */ - if (filp->f_oflags == O_WRONLY) { + /* become the publisher if we can */ + lock(); - /* become the publisher if we can */ - lock(); + if (_publisher == 0) { + _publisher = getpid(); + ret = OK; - if (_publisher == 0) { - _publisher = getpid(); - ret = OK; + } else { + ret = -EBUSY; + } - } else { - ret = -EBUSY; - } + unlock(); - unlock(); + /* now complete the open */ + if (ret == OK) { + ret = CDev::open(filp); - /* now complete the open */ - if (ret == OK) { - ret = CDev::open(filp); + /* open failed - not the publisher anymore */ + if (ret != OK) { + _publisher = 0; + } + } - /* open failed - not the publisher anymore */ - if (ret != OK) - _publisher = 0; - } + return ret; + } - return ret; - } + /* is this a new subscriber? */ + if (filp->f_oflags == O_RDONLY) { - /* is this a new subscriber? */ - if (filp->f_oflags == O_RDONLY) { + /* allocate subscriber data */ + SubscriberData *sd = new SubscriberData; - /* allocate subscriber data */ - SubscriberData *sd = new SubscriberData; + if (nullptr == sd) { + return -ENOMEM; + } - if (nullptr == sd) - return -ENOMEM; + memset(sd, 0, sizeof(*sd)); - memset(sd, 0, sizeof(*sd)); + /* default to no pending update */ + sd->generation = _generation; - /* default to no pending update */ - sd->generation = _generation; + /* set priority */ + sd->priority = _priority; - /* set priority */ - sd->priority = _priority; + filp->f_priv = (void *)sd; - filp->f_priv = (void *)sd; + ret = CDev::open(filp); - ret = CDev::open(filp); + add_internal_subscriber(); - if (ret != OK) - delete sd; + if (ret != OK) { + delete sd; + } - return ret; - } + return ret; + } - /* can only be pub or sub, not both */ - return -EINVAL; + /* can only be pub or sub, not both */ + return -EINVAL; } int uORB::DeviceNode::close(struct file *filp) { - /* is this the publisher closing? */ - if (getpid() == _publisher) { - _publisher = 0; - - } else { - SubscriberData *sd = filp_to_sd(filp); - - if (sd != nullptr) { - hrt_cancel(&sd->update_call); - delete sd; - } - } - - return CDev::close(filp); + /* is this the publisher closing? */ + if (getpid() == _publisher) { + _publisher = 0; + + } else { + SubscriberData *sd = filp_to_sd(filp); + + if (sd != nullptr) { + hrt_cancel(&sd->update_call); + remove_internal_subscriber(); + delete sd; + sd = nullptr; + } + } + + return CDev::close(filp); } ssize_t uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen) { - SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); + SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); - /* if the object has not been written yet, return zero */ - if (_data == nullptr) - return 0; + /* if the object has not been written yet, return zero */ + if (_data == nullptr) { + return 0; + } - /* if the caller's buffer is the wrong size, that's an error */ - if (buflen != _meta->o_size) - return -EIO; + /* if the caller's buffer is the wrong size, that's an error */ + if (buflen != _meta->o_size) { + return -EIO; + } - /* - * Perform an atomic copy & state update - */ - irqstate_t flags = irqsave(); + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); - /* if the caller doesn't want the data, don't give it to them */ - if (nullptr != buffer) - memcpy(buffer, _data, _meta->o_size); + /* if the caller doesn't want the data, don't give it to them */ + if (nullptr != buffer) { + memcpy(buffer, _data, _meta->o_size); + } - /* track the last generation that the file has seen */ - sd->generation = _generation; + /* track the last generation that the file has seen */ + sd->generation = _generation; - /* set priority */ - sd->priority = _priority; + /* set priority */ + sd->priority = _priority; - /* - * Clear the flag that indicates that an update has been reported, as - * we have just collected it. - */ - sd->update_reported = false; + /* + * Clear the flag that indicates that an update has been reported, as + * we have just collected it. + */ + sd->update_reported = false; - irqrestore(flags); + irqrestore(flags); - return _meta->o_size; + return _meta->o_size; } ssize_t uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen) { - /* - * Writes are legal from interrupt context as long as the - * object has already been initialised from thread context. - * - * Writes outside interrupt context will allocate the object - * if it has not yet been allocated. - * - * Note that filp will usually be NULL. - */ - if (nullptr == _data) { - if (!up_interrupt_context()) { - - lock(); - - /* re-check size */ - if (nullptr == _data) - _data = new uint8_t[_meta->o_size]; - - unlock(); - } - - /* failed or could not allocate */ - if (nullptr == _data) - return -ENOMEM; - } - - /* If write size does not match, that is an error */ - if (_meta->o_size != buflen) - return -EIO; - - /* Perform an atomic copy. */ - irqstate_t flags = irqsave(); - memcpy(_data, buffer, _meta->o_size); - irqrestore(flags); - - /* update the timestamp and generation count */ - _last_update = hrt_absolute_time(); - _generation++; - - /* notify any poll waiters */ - poll_notify(POLLIN); - - return _meta->o_size; + /* + * Writes are legal from interrupt context as long as the + * object has already been initialised from thread context. + * + * Writes outside interrupt context will allocate the object + * if it has not yet been allocated. + * + * Note that filp will usually be NULL. + */ + if (nullptr == _data) { + if (!up_interrupt_context()) { + + lock(); + + /* re-check size */ + if (nullptr == _data) { + _data = new uint8_t[_meta->o_size]; + } + + unlock(); + } + + /* failed or could not allocate */ + if (nullptr == _data) { + return -ENOMEM; + } + } + + /* If write size does not match, that is an error */ + if (_meta->o_size != buflen) { + return -EIO; + } + + /* Perform an atomic copy. */ + irqstate_t flags = irqsave(); + memcpy(_data, buffer, _meta->o_size); + irqrestore(flags); + + /* update the timestamp and generation count */ + _last_update = hrt_absolute_time(); + _generation++; + + /* notify any poll waiters */ + poll_notify(POLLIN); + + _published = true; + + return _meta->o_size; } int uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg) { - SubscriberData *sd = filp_to_sd(filp); + SubscriberData *sd = filp_to_sd(filp); - switch (cmd) { - case ORBIOCLASTUPDATE: - *(hrt_abstime *)arg = _last_update; - return OK; + switch (cmd) { + case ORBIOCLASTUPDATE: + *(hrt_abstime *)arg = _last_update; + return OK; - case ORBIOCUPDATED: - *(bool *)arg = appears_updated(sd); - return OK; + case ORBIOCUPDATED: + *(bool *)arg = appears_updated(sd); + return OK; - case ORBIOCSETINTERVAL: - sd->update_interval = arg; - return OK; + case ORBIOCSETINTERVAL: + sd->update_interval = arg; + return OK; - case ORBIOCGADVERTISER: - *(uintptr_t *)arg = (uintptr_t)this; - return OK; + case ORBIOCGADVERTISER: + *(uintptr_t *)arg = (uintptr_t)this; + return OK; - case ORBIOCGPRIORITY: - *(int *)arg = sd->priority; - return OK; + case ORBIOCGPRIORITY: + *(int *)arg = sd->priority; + return OK; - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } } ssize_t uORB::DeviceNode::publish ( - const orb_metadata *meta, - orb_advert_t handle, - const void *data + const orb_metadata *meta, + orb_advert_t handle, + const void *data ) { - uORB::DeviceNode *DeviceNode = (uORB::DeviceNode *)handle; - int ret; - - /* this is a bit risky, since we are trusting the handle in order to deref it */ - if (DeviceNode->_meta != meta) { - errno = EINVAL; - return ERROR; - } - - /* call the DeviceNode write method with no file pointer */ - ret = DeviceNode->write(nullptr, (const char *)data, meta->o_size); - - if (ret < 0) - return ERROR; - - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - - return OK; + uORB::DeviceNode *DeviceNode = (uORB::DeviceNode *)handle; + int ret; + + /* this is a bit risky, since we are trusting the handle in order to deref it */ + if (DeviceNode->_meta != meta) { + errno = EINVAL; + return ERROR; + } + + /* call the DeviceNode write method with no file pointer */ + ret = DeviceNode->write(nullptr, (const char *)data, meta->o_size); + + if (ret < 0) { + return ERROR; + } + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + /* + * if the write is successful, send the data over the Multi-ORB link + */ + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr) { + if (ch->send_message(meta->o_name, meta->o_size, (uint8_t *)data) != 0) { + warnx("[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel", + __LINE__, meta->o_name); + return ERROR; + } + } + + return OK; } pollevent_t uORB::DeviceNode::poll_state(struct file *filp) { - SubscriberData *sd = filp_to_sd(filp); + SubscriberData *sd = filp_to_sd(filp); - /* - * If the topic appears updated to the subscriber, say so. - */ - if (appears_updated(sd)) - return POLLIN; + /* + * If the topic appears updated to the subscriber, say so. + */ + if (appears_updated(sd)) { + return POLLIN; + } - return 0; + return 0; } void uORB::DeviceNode::poll_notify_one(struct pollfd *fds, pollevent_t events) { - SubscriberData *sd = filp_to_sd((struct file *)fds->priv); - - /* - * If the topic looks updated to the subscriber, go ahead and notify them. - */ - if (appears_updated(sd)) - CDev::poll_notify_one(fds, events); + SubscriberData *sd = filp_to_sd((struct file *)fds->priv); + + /* + * If the topic looks updated to the subscriber, go ahead and notify them. + */ + if (appears_updated(sd)) { + CDev::poll_notify_one(fds, events); + } } bool uORB::DeviceNode::appears_updated(SubscriberData *sd) { - /* assume it doesn't look updated */ - bool ret = false; - - /* avoid racing between interrupt and non-interrupt context calls */ - irqstate_t state = irqsave(); - - /* check if this topic has been published yet, if not bail out */ - if (_data == nullptr) { - ret = false; - goto out; - } - - /* - * If the subscriber's generation count matches the update generation - * count, there has been no update from their perspective; if they - * don't match then we might have a visible update. - */ - while (sd->generation != _generation) { - - /* - * Handle non-rate-limited subscribers. - */ - if (sd->update_interval == 0) { - ret = true; - break; - } - - /* - * If we have previously told the subscriber that there is data, - * and they have not yet collected it, continue to tell them - * that there has been an update. This mimics the non-rate-limited - * behaviour where checking / polling continues to report an update - * until the topic is read. - */ - if (sd->update_reported) { - ret = true; - break; - } - - /* - * If the interval timer is still running, the topic should not - * appear updated, even though at this point we know that it has. - * We have previously been through here, so the subscriber - * must have collected the update we reported, otherwise - * update_reported would still be true. - */ - if (!hrt_called(&sd->update_call)) - break; - - /* - * Make sure that we don't consider the topic to be updated again - * until the interval has passed once more by restarting the interval - * timer and thereby re-scheduling a poll notification at that time. - */ - hrt_call_after(&sd->update_call, - sd->update_interval, - &uORB::DeviceNode::update_deferred_trampoline, - (void *)this); - - /* - * Remember that we have told the subscriber that there is data. - */ - sd->update_reported = true; - ret = true; - - break; - } + /* assume it doesn't look updated */ + bool ret = false; + + /* avoid racing between interrupt and non-interrupt context calls */ + irqstate_t state = irqsave(); + + /* check if this topic has been published yet, if not bail out */ + if (_data == nullptr) { + ret = false; + goto out; + } + + /* + * If the subscriber's generation count matches the update generation + * count, there has been no update from their perspective; if they + * don't match then we might have a visible update. + */ + while (sd->generation != _generation) { + + /* + * Handle non-rate-limited subscribers. + */ + if (sd->update_interval == 0) { + ret = true; + break; + } + + /* + * If we have previously told the subscriber that there is data, + * and they have not yet collected it, continue to tell them + * that there has been an update. This mimics the non-rate-limited + * behaviour where checking / polling continues to report an update + * until the topic is read. + */ + if (sd->update_reported) { + ret = true; + break; + } + + /* + * If the interval timer is still running, the topic should not + * appear updated, even though at this point we know that it has. + * We have previously been through here, so the subscriber + * must have collected the update we reported, otherwise + * update_reported would still be true. + */ + if (!hrt_called(&sd->update_call)) { + break; + } + + /* + * Make sure that we don't consider the topic to be updated again + * until the interval has passed once more by restarting the interval + * timer and thereby re-scheduling a poll notification at that time. + */ + hrt_call_after(&sd->update_call, + sd->update_interval, + &uORB::DeviceNode::update_deferred_trampoline, + (void *)this); + + /* + * Remember that we have told the subscriber that there is data. + */ + sd->update_reported = true; + ret = true; + + break; + } out: - irqrestore(state); + irqrestore(state); - /* consider it updated */ - return ret; + /* consider it updated */ + return ret; } void uORB::DeviceNode::update_deferred() { - /* - * Instigate a poll notification; any subscribers whose intervals have - * expired will be woken. - */ - poll_notify(POLLIN); + /* + * Instigate a poll notification; any subscribers whose intervals have + * expired will be woken. + */ + poll_notify(POLLIN); } void uORB::DeviceNode::update_deferred_trampoline(void *arg) { - uORB::DeviceNode *node = (uORB::DeviceNode *)arg; + uORB::DeviceNode *node = (uORB::DeviceNode *)arg; + + node->update_deferred(); +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::add_internal_subscriber() +{ + _subscriber_count++; + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr && _subscriber_count > 0) { + ch->add_subscription(_meta->o_name, 1); + } +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::remove_internal_subscriber() +{ + _subscriber_count--; + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr && _subscriber_count == 0) { + ch->remove_subscription(_meta->o_name); + } +} + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +bool uORB::DeviceNode::is_published() +{ + return _published; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz) +{ + // if there is already data in the node, send this out to + // the remote entity. + // send the data to the remote entity. + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher. + ch->send_message(_meta->o_name, _meta->o_size, _data); + } + + return OK; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_remove_subscription() +{ + return OK; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data) +{ + int16_t ret = -1; - node->update_deferred(); + if (length != (int32_t)(_meta->o_size)) { + warnx("[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]", + __LINE__, _meta->o_name, (int)length, (int)_meta->o_size); + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = write(nullptr, (const char *)data, _meta->o_size); + + if (ret < 0) { + return ERROR; + } + + if (ret != (int)_meta->o_size) { + errno = EIO; + return ERROR; + } + + return OK; } uORB::DeviceMaster::DeviceMaster(Flavor f) : - CDev((f == PUBSUB) ? "obj_master" : "param_master", - (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), - _flavor(f) + CDev((f == PUBSUB) ? "obj_master" : "param_master", + (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), + _flavor(f) { - // enable debug() calls - _debug_enabled = true; + // enable debug() calls + _debug_enabled = true; } @@ -438,94 +560,126 @@ uORB::DeviceMaster::~DeviceMaster() int uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) { - int ret; - - switch (cmd) { - case ORBIOCADVERTISE: { - const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; - const struct orb_metadata *meta = adv->meta; - const char *objname; - const char *devpath; - char nodepath[orb_maxpath]; - uORB::DeviceNode *node; - - /* set instance to zero - we could allow selective multi-pubs later based on value */ - if (adv->instance != nullptr) { - *(adv->instance) = 0; - } - - /* construct a path to the node - this also checks the node name */ - ret = uORB::Utils::node_mkpath( nodepath, _flavor, meta, adv->instance); - - if (ret != OK) { - return ret; - } - - /* ensure that only one advertiser runs through this critical section */ - lock(); - - ret = ERROR; - - /* try for topic groups */ - const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; - unsigned group_tries = 0; - do { - /* if path is modifyable change try index */ - if (adv->instance != nullptr) { - /* replace the number at the end of the string */ - nodepath[strlen(nodepath) - 1] = '0' + group_tries; - *(adv->instance) = group_tries; - } - - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); - - if (objname == nullptr) { - return -ENOMEM; - } - - /* driver wants a permanent copy of the path, so make one here */ - devpath = strdup(nodepath); - - if (devpath == nullptr) { - return -ENOMEM; - } - - /* construct the new node */ - node = new uORB::DeviceNode(meta, objname, devpath, adv->priority); - - /* if we didn't get a device, that's bad */ - if (node == nullptr) { - unlock(); - return -ENOMEM; - } - - /* initialise the node - this may fail if e.g. a node with this name already exists */ - ret = node->init(); - - /* if init failed, discard the node and its name */ - if (ret != OK) { - delete node; - free((void *)objname); - free((void *)devpath); - } - - group_tries++; - - } while (ret != OK && (group_tries < max_group_tries)); - - if (group_tries > max_group_tries) { - ret = -ENOMEM; - } - - /* the file handle for the driver has been created, unlock */ - unlock(); - - return ret; - } - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } + int ret; + + switch (cmd) { + case ORBIOCADVERTISE: { + const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; + const struct orb_metadata *meta = adv->meta; + const char *objname; + const char *devpath; + char nodepath[orb_maxpath]; + uORB::DeviceNode *node; + + /* set instance to zero - we could allow selective multi-pubs later based on value */ + if (adv->instance != nullptr) { + *(adv->instance) = 0; + } + + /* construct a path to the node - this also checks the node name */ + ret = uORB::Utils::node_mkpath(nodepath, _flavor, meta, adv->instance); + + if (ret != OK) { + return ret; + } + + /* ensure that only one advertiser runs through this critical section */ + lock(); + + ret = ERROR; + + /* try for topic groups */ + const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; + + do { + /* if path is modifyable change try index */ + if (adv->instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *(adv->instance) = group_tries; + } + + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); + + if (objname == nullptr) { + return -ENOMEM; + } + + /* driver wants a permanent copy of the path, so make one here */ + devpath = strdup(nodepath); + + if (devpath == nullptr) { + return -ENOMEM; + } + + /* construct the new node */ + node = new uORB::DeviceNode(meta, objname, devpath, adv->priority); + + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + unlock(); + return -ENOMEM; + } + + /* initialise the node - this may fail if e.g. a node with this name already exists */ + ret = node->init(); + + if (ret != OK) { + /* if init failed, discard the node */ + delete node; + + if (ret == -EEXIST) { + /* if the node exists already, get the existing one and check if + * something has been published yet. */ + uORB::DeviceNode *existing_node = GetDeviceNode(devpath); + + if ((existing_node != nullptr) && !(existing_node->is_published())) { + /* nothing has been published yet, lets claim it */ + ret = OK; + + } else { + /* otherwise: data has already been published, keep looking */ + } + } + + /* also discard the name now */ + free((void *)objname); + free((void *)devpath); + + } else { + // add to the node map;. + _node_map.insert(nodepath, node); + } + + group_tries++; + + } while (ret != OK && (group_tries < max_group_tries)); + + if (group_tries > max_group_tries) { + ret = -ENOMEM; + } + + /* the file handle for the driver has been created, unlock */ + unlock(); + + return ret; + } + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +uORB::DeviceNode *uORB::DeviceMaster::GetDeviceNode(const char *nodepath) +{ + uORB::DeviceNode *rc = nullptr; + + if (_node_map.find(nodepath)) { + rc = _node_map.get(nodepath); + } + + return rc; } diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index 3d8c0f90ad07..2c9ea267193e 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -35,13 +35,16 @@ #define _uORBDevices_nuttx_hpp_ #include +#include +#include +#include "ORBMap.hpp" #include "uORBCommon.hpp" namespace uORB { - class DeviceNode; - class DeviceMaster; +class DeviceNode; +class DeviceMaster; } /** @@ -50,122 +53,175 @@ namespace uORB class uORB::DeviceNode : public device::CDev { public: - /** - * Constructor - */ - DeviceNode - ( - const struct orb_metadata *meta, - const char *name, - const char *path, - int priority - ); - - /** - * Destructor - */ - ~DeviceNode(); - - /** - * Method to create a subscriber instance and return the struct - * pointing to the subscriber as a file pointer. - */ - virtual int open(struct file *filp); - - /** - * Method to close a subscriber for this topic. - */ - virtual int close(struct file *filp); - - /** - * reads data from a subscriber node to the buffer provided. - * @param filp - * The subscriber from which the data needs to be read from. - * @param buffer - * The buffer into which the data is read into. - * @param buflen - * the length of the buffer - * @return - * ssize_t the number of bytes read. - */ - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - - /** - * writes the published data to the internal buffer to be read by - * subscribers later. - * @param filp - * the subscriber; this is not used. - * @param buffer - * The buffer for the input data - * @param buflen - * the length of the buffer. - * @return ssize_t - * The number of bytes that are written - */ - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); - - /** - * IOCTL control for the subscriber. - */ - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Method to publish a data to this node. - */ - static ssize_t publish - ( - const orb_metadata *meta, - orb_advert_t handle, - const void *data - ); + /** + * Constructor + */ + DeviceNode + ( + const struct orb_metadata *meta, + const char *name, + const char *path, + int priority + ); + + /** + * Destructor + */ + ~DeviceNode(); + + /** + * Method to create a subscriber instance and return the struct + * pointing to the subscriber as a file pointer. + */ + virtual int open(struct file *filp); + + /** + * Method to close a subscriber for this topic. + */ + virtual int close(struct file *filp); + + /** + * reads data from a subscriber node to the buffer provided. + * @param filp + * The subscriber from which the data needs to be read from. + * @param buffer + * The buffer into which the data is read into. + * @param buflen + * the length of the buffer + * @return + * ssize_t the number of bytes read. + */ + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + + /** + * writes the published data to the internal buffer to be read by + * subscribers later. + * @param filp + * the subscriber; this is not used. + * @param buffer + * The buffer for the input data + * @param buflen + * the length of the buffer. + * @return ssize_t + * The number of bytes that are written + */ + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + + /** + * IOCTL control for the subscriber. + */ + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Method to publish a data to this node. + */ + static ssize_t publish + ( + const orb_metadata *meta, + orb_advert_t handle, + const void *data + ); + + /** + * processes a request for add subscription from remote + * @param rateInHz + * Specifies the desired rate for the message. + * @return + * 0 = success + * otherwise failure. + */ + int16_t process_add_subscription(int32_t rateInHz); + + /** + * processes a request to remove a subscription from remote. + */ + int16_t process_remove_subscription(); + + /** + * processed the received data message from remote. + */ + int16_t process_received_message(int32_t length, uint8_t *data); + + /** + * Add the subscriber to the node's list of subscriber. If there is + * remote proxy to which this subscription needs to be sent, it will + * done via uORBCommunicator::IChannel interface. + * @param sd + * the subscriber to be added. + */ + void add_internal_subscriber(); + + /** + * Removes the subscriber from the list. Also notifies the remote + * if there a uORBCommunicator::IChannel instance. + * @param sd + * the Subscriber to be removed. + */ + void remove_internal_subscriber(); + + /** + * Return true if this topic has been published. + * + * This is used in the case of multi_pub/sub to check if it's valid to advertise + * and publish to this node or if another node should be tried. */ + bool is_published(); protected: - virtual pollevent_t poll_state(struct file *filp); - virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + virtual pollevent_t poll_state(struct file *filp); + virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); private: - struct SubscriberData { - unsigned generation; /**< last generation the subscriber has seen */ - unsigned update_interval; /**< if nonzero minimum interval between updates */ - struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ - void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ - bool update_reported; /**< true if we have reported the update via poll/check */ - int priority; /**< priority of publisher */ - }; - - const struct orb_metadata *_meta; /**< object metadata information */ - uint8_t *_data; /**< allocated object buffer */ - hrt_abstime _last_update; /**< time the object was last updated */ - volatile unsigned _generation; /**< object generation count */ - pid_t _publisher; /**< if nonzero, current publisher */ - const int _priority; /**< priority of topic */ + struct SubscriberData { + unsigned generation; /**< last generation the subscriber has seen */ + unsigned update_interval; /**< if nonzero minimum interval between updates */ + struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ + void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ + bool update_reported; /**< true if we have reported the update via poll/check */ + int priority; /**< priority of publisher */ + }; + + const struct orb_metadata *_meta; /**< object metadata information */ + uint8_t *_data; /**< allocated object buffer */ + hrt_abstime _last_update; /**< time the object was last updated */ + volatile unsigned _generation; /**< object generation count */ + pid_t _publisher; /**< if nonzero, current publisher */ + const int _priority; /**< priority of topic */ + bool _published; /**< has ever data been published */ private: // private class methods. - SubscriberData *filp_to_sd(struct file *filp) { - SubscriberData *sd = (SubscriberData *)(filp->f_priv); - return sd; - } - - /** - * Perform a deferred update for a rate-limited subscriber. - */ - void update_deferred(); - - /** - * Bridge from hrt_call to update_deferred - * - * void *arg ORBDevNode pointer for which the deferred update is performed. - */ - static void update_deferred_trampoline(void *arg); - - /** - * Check whether a topic appears updated to a subscriber. - * - * @param sd The subscriber for whom to check. - * @return True if the topic should appear updated to the subscriber - */ - bool appears_updated(SubscriberData *sd); + SubscriberData *filp_to_sd(struct file *filp) + { + SubscriberData *sd = (SubscriberData *)(filp->f_priv); + return sd; + } + + bool _IsRemoteSubscriberPresent; + int32_t _subscriber_count; + + /** + * Perform a deferred update for a rate-limited subscriber. + */ + void update_deferred(); + + /** + * Bridge from hrt_call to update_deferred + * + * void *arg ORBDevNode pointer for which the deferred update is performed. + */ + static void update_deferred_trampoline(void *arg); + + /** + * Check whether a topic appears updated to a subscriber. + * + * @param sd The subscriber for whom to check. + * @return True if the topic should appear updated to the subscriber + */ + bool appears_updated(SubscriberData *sd); + + // disable copy and assignment operators + DeviceNode(const DeviceNode &); + DeviceNode &operator=(const DeviceNode &); }; /** @@ -177,12 +233,14 @@ class uORB::DeviceNode : public device::CDev class uORB::DeviceMaster : public device::CDev { public: - DeviceMaster(Flavor f); - virtual ~DeviceMaster(); + DeviceMaster(Flavor f); + virtual ~DeviceMaster(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + static uORB::DeviceNode *GetDeviceNode(const char *node_name); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: - Flavor _flavor; + Flavor _flavor; + static ORBMap _node_map; }; diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index 9035a32bdc55..7c48c4ed793d 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -35,415 +35,535 @@ #include #include #include +#include #include "uORBDevices_posix.hpp" #include "uORBUtils.hpp" +#include "uORBManager.hpp" +#include "uORBCommunicator.hpp" #include +std::map uORB::DeviceMaster::_node_map; + uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(device::file_t *filp) { - uORB::DeviceNode::SubscriberData *sd; - if (filp) { - sd = (uORB::DeviceNode::SubscriberData *)(filp->priv); - } - else { - sd = 0; - } - return sd; + uORB::DeviceNode::SubscriberData *sd; + + if (filp) { + sd = (uORB::DeviceNode::SubscriberData *)(filp->priv); + + } else { + sd = 0; + } + + return sd; } uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : - VDev(name, path), - _meta(meta), - _data(nullptr), - _last_update(0), - _generation(0), - _publisher(0), - _priority(priority) + VDev(name, path), + _meta(meta), + _data(nullptr), + _last_update(0), + _generation(0), + _publisher(0), + _priority(priority), + _published(false), + _subscriber_count(0) { - // enable debug() calls - //_debug_enabled = true; + // enable debug() calls + //_debug_enabled = true; } uORB::DeviceNode::~DeviceNode() { - if (_data != nullptr) - delete[] _data; + if (_data != nullptr) { + delete[] _data; + } } int uORB::DeviceNode::open(device::file_t *filp) { - int ret; + int ret; + + /* is this a publisher? */ + if (filp->flags == PX4_F_WRONLY) { - /* is this a publisher? */ - if (filp->flags == PX4_F_WRONLY) { + /* become the publisher if we can */ + lock(); - /* become the publisher if we can */ - lock(); + if (_publisher == 0) { + _publisher = px4_getpid(); + ret = PX4_OK; - if (_publisher == 0) { - _publisher = getpid(); - ret = PX4_OK; + } else { + ret = -EBUSY; + } - } else { - ret = -EBUSY; - } + unlock(); - unlock(); + /* now complete the open */ + if (ret == PX4_OK) { + ret = VDev::open(filp); - /* now complete the open */ - if (ret == PX4_OK) { - ret = VDev::open(filp); + /* open failed - not the publisher anymore */ + if (ret != PX4_OK) { + _publisher = 0; + } + } - /* open failed - not the publisher anymore */ - if (ret != PX4_OK) - _publisher = 0; - } + return ret; + } - return ret; - } + /* is this a new subscriber? */ + if (filp->flags == PX4_F_RDONLY) { - /* is this a new subscriber? */ - if (filp->flags == PX4_F_RDONLY) { + /* allocate subscriber data */ + SubscriberData *sd = new SubscriberData; - /* allocate subscriber data */ - SubscriberData *sd = new SubscriberData; + if (nullptr == sd) { + return -ENOMEM; + } - if (nullptr == sd) - return -ENOMEM; + memset(sd, 0, sizeof(*sd)); - memset(sd, 0, sizeof(*sd)); + /* default to no pending update */ + sd->generation = _generation; - /* default to no pending update */ - sd->generation = _generation; + /* set priority */ + sd->priority = _priority; - /* set priority */ - sd->priority = _priority; + filp->priv = (void *)sd; - filp->priv = (void *)sd; + ret = VDev::open(filp); - ret = VDev::open(filp); + add_internal_subscriber(); - if (ret != PX4_OK) { - warnx("ERROR: VDev::open failed\n"); - delete sd; - } + if (ret != PX4_OK) { + warnx("ERROR: VDev::open failed\n"); + delete sd; + } - //warnx("uORB::DeviceNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", filp->fd, filp->flags, filp->priv, filp->cdev); - return ret; - } + //warnx("uORB::DeviceNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", filp->fd, filp->flags, filp->priv, filp->cdev); + return ret; + } - /* can only be pub or sub, not both */ - return -EINVAL; + /* can only be pub or sub, not both */ + return -EINVAL; } int uORB::DeviceNode::close(device::file_t *filp) { - //warnx("uORB::DeviceNode::close fd = %d", filp->fd); - /* is this the publisher closing? */ - if (getpid() == _publisher) { - _publisher = 0; - - } else { - SubscriberData *sd = filp_to_sd(filp); - - if (sd != nullptr) { - hrt_cancel(&sd->update_call); - delete sd; - } - } - - return VDev::close(filp); + //warnx("uORB::DeviceNode::close fd = %d", filp->fd); + /* is this the publisher closing? */ + if (px4_getpid() == _publisher) { + _publisher = 0; + + } else { + SubscriberData *sd = filp_to_sd(filp); + + if (sd != nullptr) { + hrt_cancel(&sd->update_call); + remove_internal_subscriber(); + delete sd; + sd = nullptr; + } + } + + return VDev::close(filp); } ssize_t uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen) { - //warnx("uORB::DeviceNode::read fd = %d\n", filp->fd); - SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); + //warnx("uORB::DeviceNode::read fd = %d\n", filp->fd); + SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); - /* if the object has not been written yet, return zero */ - if (_data == nullptr) - return 0; + /* if the object has not been written yet, return zero */ + if (_data == nullptr) { + return 0; + } - /* if the caller's buffer is the wrong size, that's an error */ - if (buflen != _meta->o_size) - return -EIO; + /* if the caller's buffer is the wrong size, that's an error */ + if (buflen != _meta->o_size) { + return -EIO; + } - /* - * Perform an atomic copy & state update - */ - lock(); + /* + * Perform an atomic copy & state update + */ + lock(); - /* if the caller doesn't want the data, don't give it to them */ - if (nullptr != buffer) - memcpy(buffer, _data, _meta->o_size); + /* if the caller doesn't want the data, don't give it to them */ + if (nullptr != buffer) { + memcpy(buffer, _data, _meta->o_size); + } - /* track the last generation that the file has seen */ - sd->generation = _generation; + /* track the last generation that the file has seen */ + sd->generation = _generation; - /* set priority */ - sd->priority = _priority; + /* set priority */ + sd->priority = _priority; - /* - * Clear the flag that indicates that an update has been reported, as - * we have just collected it. - */ - sd->update_reported = false; + /* + * Clear the flag that indicates that an update has been reported, as + * we have just collected it. + */ + sd->update_reported = false; - unlock(); + unlock(); - return _meta->o_size; + return _meta->o_size; } ssize_t uORB::DeviceNode::write(device::file_t *filp, const char *buffer, size_t buflen) { - //warnx("uORB::DeviceNode::write filp = %p (null is normal)", filp); - /* - * Writes are legal from interrupt context as long as the - * object has already been initialised from thread context. - * - * Writes outside interrupt context will allocate the object - * if it has not yet been allocated. - * - * Note that filp will usually be NULL. - */ - if (nullptr == _data) { - lock(); - - /* re-check size */ - if (nullptr == _data) - _data = new uint8_t[_meta->o_size]; - - unlock(); - - /* failed or could not allocate */ - if (nullptr == _data) - return -ENOMEM; - } - - /* If write size does not match, that is an error */ - if (_meta->o_size != buflen) - return -EIO; - - /* Perform an atomic copy. */ - lock(); - memcpy(_data, buffer, _meta->o_size); - unlock(); - - /* update the timestamp and generation count */ - _last_update = hrt_absolute_time(); - _generation++; - - /* notify any poll waiters */ - poll_notify(POLLIN); - - return _meta->o_size; + //warnx("uORB::DeviceNode::write filp = %p (null is normal)", filp); + /* + * Writes are legal from interrupt context as long as the + * object has already been initialised from thread context. + * + * Writes outside interrupt context will allocate the object + * if it has not yet been allocated. + * + * Note that filp will usually be NULL. + */ + if (nullptr == _data) { + lock(); + + /* re-check size */ + if (nullptr == _data) { + _data = new uint8_t[_meta->o_size]; + } + + unlock(); + + /* failed or could not allocate */ + if (nullptr == _data) { + return -ENOMEM; + } + } + + /* If write size does not match, that is an error */ + if (_meta->o_size != buflen) { + return -EIO; + } + + /* Perform an atomic copy. */ + lock(); + memcpy(_data, buffer, _meta->o_size); + unlock(); + + /* update the timestamp and generation count */ + _last_update = hrt_absolute_time(); + _generation++; + + /* notify any poll waiters */ + poll_notify(POLLIN); + + _published = true; + + return _meta->o_size; } int uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) { - //warnx("uORB::DeviceNode::ioctl fd = %d cmd = %d", filp->fd, cmd); - SubscriberData *sd = filp_to_sd(filp); - - switch (cmd) { - case ORBIOCLASTUPDATE: - *(hrt_abstime *)arg = _last_update; - return PX4_OK; - - case ORBIOCUPDATED: - *(bool *)arg = appears_updated(sd); - return PX4_OK; - - case ORBIOCSETINTERVAL: - sd->update_interval = arg; - return PX4_OK; - - case ORBIOCGADVERTISER: - *(uintptr_t *)arg = (uintptr_t)this; - return PX4_OK; - - case ORBIOCGPRIORITY: - *(int *)arg = sd->priority; - return PX4_OK; - - default: - /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); - } + //warnx("uORB::DeviceNode::ioctl fd = %d cmd = %d", filp->fd, cmd); + SubscriberData *sd = filp_to_sd(filp); + + switch (cmd) { + case ORBIOCLASTUPDATE: + *(hrt_abstime *)arg = _last_update; + return PX4_OK; + + case ORBIOCUPDATED: + *(bool *)arg = appears_updated(sd); + return PX4_OK; + + case ORBIOCSETINTERVAL: + sd->update_interval = arg; + return PX4_OK; + + case ORBIOCGADVERTISER: + *(uintptr_t *)arg = (uintptr_t)this; + return PX4_OK; + + case ORBIOCGPRIORITY: + *(int *)arg = sd->priority; + return PX4_OK; + + default: + /* give it to the superclass */ + return VDev::ioctl(filp, cmd, arg); + } } ssize_t uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) { - //warnx("uORB::DeviceNode::publish meta = %p", meta); - - if (handle == nullptr) { - warnx("uORB::DeviceNode::publish called with invalid handle"); - errno = EINVAL; - return ERROR; - } - - uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle; - int ret; - - /* this is a bit risky, since we are trusting the handle in order to deref it */ - if (devnode->_meta != meta) { - errno = EINVAL; - return ERROR; - } - - /* call the devnode write method with no file pointer */ - ret = devnode->write(nullptr, (const char *)data, meta->o_size); - - if (ret < 0) - return ERROR; - - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - - return PX4_OK; + //warnx("uORB::DeviceNode::publish meta = %p", meta); + + if (handle == nullptr) { + warnx("uORB::DeviceNode::publish called with invalid handle"); + errno = EINVAL; + return ERROR; + } + + uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle; + int ret; + + /* this is a bit risky, since we are trusting the handle in order to deref it */ + if (devnode->_meta != meta) { + errno = EINVAL; + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = devnode->write(nullptr, (const char *)data, meta->o_size); + + if (ret < 0) { + return ERROR; + } + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + /* + * if the write is successful, send the data over the Multi-ORB link + */ + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr) { + if (ch->send_message(meta->o_name, meta->o_size, (uint8_t *)data) != 0) { + warnx("[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel", + __LINE__, meta->o_name); + return ERROR; + } + } + + return PX4_OK; } pollevent_t uORB::DeviceNode::poll_state(device::file_t *filp) { - //warnx("uORB::DeviceNode::poll_state fd = %d", filp->fd); - SubscriberData *sd = filp_to_sd(filp); + //warnx("uORB::DeviceNode::poll_state fd = %d", filp->fd); + SubscriberData *sd = filp_to_sd(filp); - /* - * If the topic appears updated to the subscriber, say so. - */ - if (appears_updated(sd)) - return POLLIN; + /* + * If the topic appears updated to the subscriber, say so. + */ + if (appears_updated(sd)) { + return POLLIN; + } - return 0; + return 0; } void uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) { - //warnx("uORB::DeviceNode::poll_notify_one fds = %p fds->priv = %p", fds, fds->priv); - SubscriberData *sd = filp_to_sd((device::file_t *)fds->priv); - - /* - * If the topic looks updated to the subscriber, go ahead and notify them. - */ - if (appears_updated(sd)) - VDev::poll_notify_one(fds, events); + //warnx("uORB::DeviceNode::poll_notify_one fds = %p fds->priv = %p", fds, fds->priv); + SubscriberData *sd = filp_to_sd((device::file_t *)fds->priv); + + /* + * If the topic looks updated to the subscriber, go ahead and notify them. + */ + if (appears_updated(sd)) { + VDev::poll_notify_one(fds, events); + } } bool uORB::DeviceNode::appears_updated(SubscriberData *sd) { - //warnx("uORB::DeviceNode::appears_updated sd = %p", sd); - /* assume it doesn't look updated */ - bool ret = false; - - /* check if this topic has been published yet, if not bail out */ - if (_data == nullptr) { - ret = false; - goto out; - } - - /* - * If the subscriber's generation count matches the update generation - * count, there has been no update from their perspective; if they - * don't match then we might have a visible update. - */ - while (sd->generation != _generation) { - - /* - * Handle non-rate-limited subscribers. - */ - if (sd->update_interval == 0) { - ret = true; - break; - } - - /* - * If we have previously told the subscriber that there is data, - * and they have not yet collected it, continue to tell them - * that there has been an update. This mimics the non-rate-limited - * behaviour where checking / polling continues to report an update - * until the topic is read. - */ - if (sd->update_reported) { - ret = true; - break; - } - -// FIXME - the calls to hrt_called and hrt_call_after seem not to work in the -// POSIX build -#ifndef __PX4_POSIX - /* - * If the interval timer is still running, the topic should not - * appear updated, even though at this point we know that it has. - * We have previously been through here, so the subscriber - * must have collected the update we reported, otherwise - * update_reported would still be true. - */ - if (!hrt_called(&sd->update_call)) - break; - - /* - * Make sure that we don't consider the topic to be updated again - * until the interval has passed once more by restarting the interval - * timer and thereby re-scheduling a poll notification at that time. - */ - hrt_call_after(&sd->update_call, - sd->update_interval, - &uORB::DeviceNode::update_deferred_trampoline, - (void *)this); -#endif - - /* - * Remember that we have told the subscriber that there is data. - */ - sd->update_reported = true; - ret = true; - - break; - } + //warnx("uORB::DeviceNode::appears_updated sd = %p", sd); + /* assume it doesn't look updated */ + bool ret = false; + + /* check if this topic has been published yet, if not bail out */ + if (_data == nullptr) { + ret = false; + goto out; + } + + /* + * If the subscriber's generation count matches the update generation + * count, there has been no update from their perspective; if they + * don't match then we might have a visible update. + */ + while (sd->generation != _generation) { + + /* + * Handle non-rate-limited subscribers. + */ + if (sd->update_interval == 0) { + ret = true; + break; + } + + /* + * If we have previously told the subscriber that there is data, + * and they have not yet collected it, continue to tell them + * that there has been an update. This mimics the non-rate-limited + * behaviour where checking / polling continues to report an update + * until the topic is read. + */ + if (sd->update_reported) { + ret = true; + break; + } + + /* + * If the interval timer is still running, the topic should not + * appear updated, even though at this point we know that it has. + * We have previously been through here, so the subscriber + * must have collected the update we reported, otherwise + * update_reported would still be true. + */ + if (!hrt_called(&sd->update_call)) { + break; + } + + /* + * Make sure that we don't consider the topic to be updated again + * until the interval has passed once more by restarting the interval + * timer and thereby re-scheduling a poll notification at that time. + */ + hrt_call_after(&sd->update_call, + sd->update_interval, + &uORB::DeviceNode::update_deferred_trampoline, + (void *)this); + + /* + * Remember that we have told the subscriber that there is data. + */ + sd->update_reported = true; + ret = true; + + break; + } out: - /* consider it updated */ - return ret; + /* consider it updated */ + return ret; } void uORB::DeviceNode::update_deferred() { - /* - * Instigate a poll notification; any subscribers whose intervals have - * expired will be woken. - */ - poll_notify(POLLIN); + /* + * Instigate a poll notification; any subscribers whose intervals have + * expired will be woken. + */ + poll_notify(POLLIN); } void uORB::DeviceNode::update_deferred_trampoline(void *arg) { - uORB::DeviceNode *node = (uORB::DeviceNode *)arg; + uORB::DeviceNode *node = (uORB::DeviceNode *)arg; - node->update_deferred(); + node->update_deferred(); } +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::add_internal_subscriber() +{ + _subscriber_count++; + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr && _subscriber_count > 0) { + ch->add_subscription(_meta->o_name, 1); + } +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::remove_internal_subscriber() +{ + _subscriber_count--; + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (ch != nullptr && _subscriber_count == 0) { + ch->remove_subscription(_meta->o_name); + } +} + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +bool uORB::DeviceNode::is_published() +{ + return _published; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz) +{ + // if there is already data in the node, send this out to + // the remote entity. + // send the data to the remote entity. + uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + + if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher. + ch->send_message(_meta->o_name, _meta->o_size, _data); + } + + return 0; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_remove_subscription() +{ + return 0; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data) +{ + int16_t ret = -1; + + if (length != (int32_t)(_meta->o_size)) { + warnx("[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]", + __LINE__, _meta->o_name, (int)length, (int)_meta->o_size); + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = write(nullptr, (const char *)data, _meta->o_size); + + if (ret < 0) { + return ERROR; + } + + if (ret != (int)_meta->o_size) { + errno = EIO; + return ERROR; + } + + return PX4_OK; +} + + uORB::DeviceMaster::DeviceMaster(Flavor f) : - VDev((f == PUBSUB) ? "obj_master" : "param_master", - (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), - _flavor(f) + VDev((f == PUBSUB) ? "obj_master" : "param_master", + (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), + _flavor(f) { - // enable debug() calls - //_debug_enabled = true; + // enable debug() calls + //_debug_enabled = true; } @@ -454,98 +574,131 @@ uORB::DeviceMaster::~DeviceMaster() int uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) { - int ret; - - switch (cmd) { - case ORBIOCADVERTISE: { - const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; - const struct orb_metadata *meta = adv->meta; - const char *objname; - const char *devpath; - char nodepath[orb_maxpath]; - uORB::DeviceNode *node; - - /* set instance to zero - we could allow selective multi-pubs later based on value */ - if (adv->instance != nullptr) { - *(adv->instance) = 0; - } - - /* construct a path to the node - this also checks the node name */ - ret = uORB::Utils::node_mkpath(nodepath, _flavor, meta, adv->instance); - - if (ret != PX4_OK) { - return ret; - } - - /* ensure that only one advertiser runs through this critical section */ - lock(); - - ret = ERROR; - - /* try for topic groups */ - const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; - unsigned group_tries = 0; - do { - /* if path is modifyable change try index */ - if (adv->instance != nullptr) { - /* replace the number at the end of the string */ - nodepath[strlen(nodepath) - 1] = '0' + group_tries; - *(adv->instance) = group_tries; - } - - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); - - if (objname == nullptr) { - return -ENOMEM; - } - - /* driver wants a permanent copy of the path, so make one here */ - devpath = strdup(nodepath); - - if (devpath == nullptr) { - // FIXME - looks like we leaked memory here for objname - return -ENOMEM; - } - - /* construct the new node */ - node = new uORB::DeviceNode(meta, objname, devpath, adv->priority); - - /* if we didn't get a device, that's bad */ - if (node == nullptr) { - unlock(); - - // FIXME - looks like we leaked memory here for devpath and objname - return -ENOMEM; - } - - /* initialise the node - this may fail if e.g. a node with this name already exists */ - ret = node->init(); - - /* if init failed, discard the node and its name */ - if (ret != PX4_OK) { - delete node; - free((void *)objname); - free((void *)devpath); - } - - group_tries++; - - } while (ret != PX4_OK && (group_tries < max_group_tries)); - - if (group_tries > max_group_tries) { - ret = -ENOMEM; - } - - /* the file handle for the driver has been created, unlock */ - unlock(); - - return ret; - } - - default: - /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); - } + int ret; + + switch (cmd) { + case ORBIOCADVERTISE: { + const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; + const struct orb_metadata *meta = adv->meta; + const char *objname; + const char *devpath; + char nodepath[orb_maxpath]; + uORB::DeviceNode *node; + + /* set instance to zero - we could allow selective multi-pubs later based on value */ + if (adv->instance != nullptr) { + *(adv->instance) = 0; + } + + /* construct a path to the node - this also checks the node name */ + ret = uORB::Utils::node_mkpath(nodepath, _flavor, meta, adv->instance); + + if (ret != PX4_OK) { + return ret; + } + + /* ensure that only one advertiser runs through this critical section */ + lock(); + + ret = ERROR; + + /* try for topic groups */ + const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; + + do { + /* if path is modifyable change try index */ + if (adv->instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *(adv->instance) = group_tries; + } + + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); + + if (objname == nullptr) { + return -ENOMEM; + } + + /* driver wants a permanent copy of the path, so make one here */ + devpath = strdup(nodepath); + + if (devpath == nullptr) { + // FIXME - looks like we leaked memory here for objname + return -ENOMEM; + } + + /* construct the new node */ + node = new uORB::DeviceNode(meta, objname, devpath, adv->priority); + + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + unlock(); + + // FIXME - looks like we leaked memory here for devpath and objname + return -ENOMEM; + } + + /* initialise the node - this may fail if e.g. a node with this name already exists */ + ret = node->init(); + + /* if init failed, discard the node and its name */ + if (ret != PX4_OK) { + delete node; + + if (ret == -EEXIST) { + /* if the node exists already, get the existing one and check if + * something has been published yet. */ + uORB::DeviceNode *existing_node = GetDeviceNode(devpath); + + if ((existing_node != nullptr) && !(existing_node->is_published())) { + /* nothing has been published yet, lets claim it */ + ret = PX4_OK; + + } else { + /* otherwise: data has already been published, keep looking */ + } + } + + /* also discard the name now */ + free((void *)objname); + free((void *)devpath); + + } else { + // add to the node map;. + _node_map[std::string(nodepath)] = node; + } + + + group_tries++; + + } while (ret != PX4_OK && (group_tries < max_group_tries)); + + if (group_tries > max_group_tries) { + ret = -ENOMEM; + } + + /* the file handle for the driver has been created, unlock */ + unlock(); + + return ret; + } + + default: + /* give it to the superclass */ + return VDev::ioctl(filp, cmd, arg); + } } +uORB::DeviceNode *uORB::DeviceMaster::GetDeviceNode(const char *nodepath) +{ + uORB::DeviceNode *rc = nullptr; + std::string np(nodepath); + + if (_node_map.find(np) != _node_map.end()) { + rc = _node_map[np]; + } + + return rc; +} diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index ce22b84321c1..4d45ea8431fc 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -35,71 +35,124 @@ #define _uORBDevices_posix_hpp_ #include +#include +#include #include "uORBCommon.hpp" - namespace uORB { - class DeviceNode; - class DeviceMaster; +class DeviceNode; +class DeviceMaster; } class uORB::DeviceNode : public device::VDev { public: - DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); - ~DeviceNode(); - - virtual int open(device::file_t *filp); - virtual int close(device::file_t *filp); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual ssize_t write(device::file_t *filp, const char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - - static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); + DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); + ~DeviceNode(); + + virtual int open(device::file_t *filp); + virtual int close(device::file_t *filp); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual ssize_t write(device::file_t *filp, const char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + + static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); + + /** + * processes a request for add subscription from remote + * @param rateInHz + * Specifies the desired rate for the message. + * @return + * 0 = success + * otherwise failure. + */ + int16_t process_add_subscription(int32_t rateInHz); + + /** + * processes a request to remove a subscription from remote. + */ + int16_t process_remove_subscription(); + + /** + * processed the received data message from remote. + */ + int16_t process_received_message(int32_t length, uint8_t *data); + + /** + * Add the subscriber to the node's list of subscriber. If there is + * remote proxy to which this subscription needs to be sent, it will + * done via uORBCommunicator::IChannel interface. + * @param sd + * the subscriber to be added. + */ + void add_internal_subscriber(); + + /** + * Removes the subscriber from the list. Also notifies the remote + * if there a uORBCommunicator::IChannel instance. + * @param sd + * the Subscriber to be removed. + */ + void remove_internal_subscriber(); + + /** + * Return true if this topic has been published. + * + * This is used in the case of multi_pub/sub to check if it's valid to advertise + * and publish to this node or if another node should be tried. */ + bool is_published(); protected: - virtual pollevent_t poll_state(device::file_t *filp); - virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); + virtual pollevent_t poll_state(device::file_t *filp); + virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); private: - struct SubscriberData { - unsigned generation; /**< last generation the subscriber has seen */ - unsigned update_interval; /**< if nonzero minimum interval between updates */ - struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ - void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ - bool update_reported; /**< true if we have reported the update via poll/check */ - int priority; /**< priority of publisher */ - }; - - const struct orb_metadata *_meta; /**< object metadata information */ - uint8_t *_data; /**< allocated object buffer */ - hrt_abstime _last_update; /**< time the object was last updated */ - volatile unsigned _generation; /**< object generation count */ - pid_t _publisher; /**< if nonzero, current publisher */ - const int _priority; /**< priority of topic */ - - SubscriberData *filp_to_sd(device::file_t *filp); - - /** - * Perform a deferred update for a rate-limited subscriber. - */ - void update_deferred(); - - /** - * Bridge from hrt_call to update_deferred - * - * void *arg ORBDevNode pointer for which the deferred update is performed. - */ - static void update_deferred_trampoline(void *arg); - - /** - * Check whether a topic appears updated to a subscriber. - * - * @param sd The subscriber for whom to check. - * @return True if the topic should appear updated to the subscriber - */ - bool appears_updated(SubscriberData *sd); + struct SubscriberData { + unsigned generation; /**< last generation the subscriber has seen */ + unsigned update_interval; /**< if nonzero minimum interval between updates */ + struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ + void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ + bool update_reported; /**< true if we have reported the update via poll/check */ + int priority; /**< priority of publisher */ + }; + + const struct orb_metadata *_meta; /**< object metadata information */ + uint8_t *_data; /**< allocated object buffer */ + hrt_abstime _last_update; /**< time the object was last updated */ + volatile unsigned _generation; /**< object generation count */ + unsigned long _publisher; /**< if nonzero, current publisher */ + const int _priority; /**< priority of topic */ + bool _published; /**< has ever data been published */ + + SubscriberData *filp_to_sd(device::file_t *filp); + + int32_t _subscriber_count; + + /** + * Perform a deferred update for a rate-limited subscriber. + */ + void update_deferred(); + + /** + * Bridge from hrt_call to update_deferred + * + * void *arg ORBDevNode pointer for which the deferred update is performed. + */ + static void update_deferred_trampoline(void *arg); + + /** + * Check whether a topic appears updated to a subscriber. + * + * @param sd The subscriber for whom to check. + * @return True if the topic should appear updated to the subscriber + */ + bool appears_updated(SubscriberData *sd); + + + // disable copy and assignment operators + DeviceNode(const DeviceNode &); + DeviceNode &operator=(const DeviceNode &); }; /** @@ -111,12 +164,15 @@ class uORB::DeviceNode : public device::VDev class uORB::DeviceMaster : public device::VDev { public: - DeviceMaster(Flavor f); - ~DeviceMaster(); + DeviceMaster(Flavor f); + ~DeviceMaster(); + + static uORB::DeviceNode *GetDeviceNode(const char *node_name); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); private: - Flavor _flavor; + Flavor _flavor; + static std::map _node_map; }; #endif /* _uORBDeviceNode_posix.hpp */ diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index 147ce5fbb30f..360b95a724eb 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/modules/uORB/uORBMain.cpp @@ -34,89 +34,97 @@ #include #include "uORBDevices.hpp" #include "uORB.h" -#include "uORBTest_UnitTest.hpp" #include "uORBCommon.hpp" +#ifndef __PX4_QURT +#include "uORBTest_UnitTest.hpp" +#endif + extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } static uORB::DeviceMaster *g_dev = nullptr; static void usage() { - warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'"); + warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'"); } int uorb_main(int argc, char *argv[]) { - if (argc < 2) { - usage(); - return -EINVAL; - } - - /* - * Start/load the driver. - * - * XXX it would be nice to have a wrapper for this... - */ - if (!strcmp(argv[1], "start")) { - - if (g_dev != nullptr) { - warnx("already loaded"); - /* user wanted to start uorb, its already running, no error */ - return 0; - } - - /* create the driver */ - g_dev = new uORB::DeviceMaster(uORB::PUBSUB); - - if (g_dev == nullptr) { - warnx("driver alloc failed"); - return -ENOMEM; - } - - if (OK != g_dev->init()) { - warnx("driver init failed"); - delete g_dev; - g_dev = nullptr; - return -EIO; - } - - return OK; - } - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - { - uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); - return t.test(); - } - - /* - * Test the latency. - */ - if (!strcmp(argv[1], "latency_test")) { - - uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); - if (argc > 2 && !strcmp(argv[2], "medium")) { - return t.latency_test(ORB_ID(orb_test_medium), true); - } else if (argc > 2 && !strcmp(argv[2], "large")) { - return t.latency_test(ORB_ID(orb_test_large), true); - } else { - return t.latency_test(ORB_ID(orb_test), true); - } - } - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "status")) - { - return OK; - } - - usage(); - return -EINVAL; + if (argc < 2) { + usage(); + return -EINVAL; + } + + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + + if (g_dev != nullptr) { + warnx("already loaded"); + /* user wanted to start uorb, its already running, no error */ + return 0; + } + + /* create the driver */ + g_dev = new uORB::DeviceMaster(uORB::PUBSUB); + + if (g_dev == nullptr) { + warnx("driver alloc failed"); + return -ENOMEM; + } + + if (OK != g_dev->init()) { + warnx("driver init failed"); + delete g_dev; + g_dev = nullptr; + return -EIO; + } + + return OK; + } + +#ifndef __PX4_QURT + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); + return t.test(); + } + + /* + * Test the latency. + */ + if (!strcmp(argv[1], "latency_test")) { + + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); + + if (argc > 2 && !strcmp(argv[2], "medium")) { + return t.latency_test(ORB_ID(orb_test_medium), true); + + } else if (argc > 2 && !strcmp(argv[2], "large")) { + return t.latency_test(ORB_ID(orb_test_large), true); + + } else { + return t.latency_test(ORB_ID(orb_test), true); + } + } + +#endif + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "status")) { + return OK; + } + + usage(); + return -EINVAL; } diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 05626de72389..04158234bdfe 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -35,11 +35,21 @@ #define _uORBManager_hpp_ #include "uORBCommon.hpp" +#include "uORBDevices.hpp" #include +#ifdef __PX4_NUTTX +#include "ORBSet.hpp" +#else +#include +#include +#define ORBSet std::set +#endif + +#include "uORBCommunicator.hpp" namespace uORB { - class Manager; +class Manager; } /** @@ -47,278 +57,346 @@ namespace uORB * uORB nodes for each uORB topics and also implements the behavor of the * uORB Api's. */ -class uORB::Manager +class uORB::Manager : public uORBCommunicator::IChannelRxHandler { - public: - // public interfaces for this class. +public: + // public interfaces for this class. + + /** + * Method to get the singleton instance for the uORB::Manager. + * @return uORB::Manager* + */ + static uORB::Manager *get_instance(); + + // ==== uORB interface methods ==== + /** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @return nullptr on error, otherwise returns an object pointer + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return nullptr and set errno to ENOENT. + */ + orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data); + + /** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @param instance Pointer to an integer which will yield the instance ID (0-based) + * of the publication. + * @param priority The priority of the instance. If a subscriber subscribes multiple + * instances, the priority allows the subscriber to prioritize the best + * data source as long as its available. + * @return ERROR on error, otherwise returns a handle + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ + orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, + int priority) ; + + + /** + * Publish new data to a topic. + * + * The data is atomically published to the topic and any waiting subscribers + * will be notified. Subscribers that are not waiting can check the topic + * for updates using orb_check and/or orb_stat. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @handle The handle returned from orb_advertise. + * @param data A pointer to the data to be published. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ + int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) ; + + /** + * Subscribe to a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ + int orb_subscribe(const struct orb_metadata *meta) ; - /** - * Method to get the singleton instance for the uORB::Manager. - * @return uORB::Manager* - */ - static uORB::Manager* get_instance(); + /** + * Subscribe to a multi-instance of a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param instance The instance of the topic. Instance 0 matches the + * topic of the orb_subscribe() call, higher indices + * are for topics created with orb_publish_multi(). + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ + int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) ; - // ==== uORB interface methods ==== - /** - * Advertise as the publisher of a topic. - * - * This performs the initial advertisement of a topic; it creates the topic - * node in /obj if required and publishes the initial data. - * - * Any number of advertisers may publish to a topic; publications are atomic - * but co-ordination between publishers is not provided by the ORB. - * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @param data A pointer to the initial data to be published. - * For topics updated by interrupt handlers, the advertisement - * must be performed from non-interrupt context. - * @return nullptr on error, otherwise returns an object pointer - * that can be used to publish to the topic. - * If the topic in question is not known (due to an - * ORB_DEFINE with no corresponding ORB_DECLARE) - * this function will return nullptr and set errno to ENOENT. - */ - orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data); + /** + * Unsubscribe from a topic. + * + * @param handle A handle returned from orb_subscribe. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ + int orb_unsubscribe(int handle) ; - /** - * Advertise as the publisher of a topic. - * - * This performs the initial advertisement of a topic; it creates the topic - * node in /obj if required and publishes the initial data. - * - * Any number of advertisers may publish to a topic; publications are atomic - * but co-ordination between publishers is not provided by the ORB. - * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @param data A pointer to the initial data to be published. - * For topics updated by interrupt handlers, the advertisement - * must be performed from non-interrupt context. - * @param instance Pointer to an integer which will yield the instance ID (0-based) - * of the publication. - * @param priority The priority of the instance. If a subscriber subscribes multiple - * instances, the priority allows the subscriber to prioritize the best - * data source as long as its available. - * @return ERROR on error, otherwise returns a handle - * that can be used to publish to the topic. - * If the topic in question is not known (due to an - * ORB_DEFINE with no corresponding ORB_DECLARE) - * this function will return -1 and set errno to ENOENT. - */ - orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - int priority) ; + /** + * Fetch data from a topic. + * + * This is the only operation that will reset the internal marker that + * indicates that a topic has been updated for a subscriber. Once poll + * or check return indicating that an updaet is available, this call + * must be used to update the subscription. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param handle A handle returned from orb_subscribe. + * @param buffer Pointer to the buffer receiving the data, or NULL + * if the caller wants to clear the updated flag without + * using the data. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ + int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) ; + /** + * Check whether a topic has been published to since the last orb_copy. + * + * This check can be used to determine whether to copy the topic when + * not using poll(), or to avoid the overhead of calling poll() when the + * topic is likely to have updated. + * + * Updates are tracked on a per-handle basis; this call will continue to + * return true until orb_copy is called using the same handle. This interface + * should be preferred over calling orb_stat due to the race window between + * stat and copy that can lead to missed updates. + * + * @param handle A handle returned from orb_subscribe. + * @param updated Set to true if the topic has been updated since the + * last time it was copied using this handle. + * @return OK if the check was successful, ERROR otherwise with + * errno set accordingly. + */ + int orb_check(int handle, bool *updated) ; - /** - * Publish new data to a topic. - * - * The data is atomically published to the topic and any waiting subscribers - * will be notified. Subscribers that are not waiting can check the topic - * for updates using orb_check and/or orb_stat. - * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @handle The handle returned from orb_advertise. - * @param data A pointer to the data to be published. - * @return OK on success, ERROR otherwise with errno set accordingly. - */ - int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) ; + /** + * Return the last time that the topic was updated. + * + * @param handle A handle returned from orb_subscribe. + * @param time Returns the absolute time that the topic was updated, or zero if it has + * never been updated. Time is measured in microseconds. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ + int orb_stat(int handle, uint64_t *time) ; - /** - * Subscribe to a topic. - * - * The returned value is a file descriptor that can be passed to poll() - * in order to wait for updates to a topic, as well as topic_read, - * orb_check and orb_stat. - * - * Subscription will succeed even if the topic has not been advertised; - * in this case the topic will have a timestamp of zero, it will never - * signal a poll() event, checking will always return false and it cannot - * be copied. When the topic is subsequently advertised, poll, check, - * stat and copy calls will react to the initial publication that is - * performed as part of the advertisement. - * - * Subscription will fail if the topic is not known to the system, i.e. - * there is nothing in the system that has declared the topic and thus it - * can never be published. - * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @return ERROR on error, otherwise returns a handle - * that can be used to read and update the topic. - * If the topic in question is not known (due to an - * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) - * this function will return -1 and set errno to ENOENT. - */ - int orb_subscribe(const struct orb_metadata *meta) ; + /** + * Check if a topic has already been created. + * + * @param meta ORB topic metadata. + * @param instance ORB instance + * @return OK if the topic exists, ERROR otherwise with errno set accordingly. + */ + int orb_exists(const struct orb_metadata *meta, int instance) ; - /** - * Subscribe to a multi-instance of a topic. - * - * The returned value is a file descriptor that can be passed to poll() - * in order to wait for updates to a topic, as well as topic_read, - * orb_check and orb_stat. - * - * Subscription will succeed even if the topic has not been advertised; - * in this case the topic will have a timestamp of zero, it will never - * signal a poll() event, checking will always return false and it cannot - * be copied. When the topic is subsequently advertised, poll, check, - * stat and copy calls will react to the initial publication that is - * performed as part of the advertisement. - * - * Subscription will fail if the topic is not known to the system, i.e. - * there is nothing in the system that has declared the topic and thus it - * can never be published. - * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @param instance The instance of the topic. Instance 0 matches the - * topic of the orb_subscribe() call, higher indices - * are for topics created with orb_publish_multi(). - * @return ERROR on error, otherwise returns a handle - * that can be used to read and update the topic. - * If the topic in question is not known (due to an - * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) - * this function will return -1 and set errno to ENOENT. - */ - int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) ; + /** + * Return the priority of the topic + * + * @param handle A handle returned from orb_subscribe. + * @param priority Returns the priority of this topic. This is only relevant for + * topics which are published by multiple publishers (e.g. mag0, mag1, etc.) + * and allows a subscriber to automatically pick the topic with the highest + * priority, independent of the startup order of the associated publishers. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ + int orb_priority(int handle, int32_t *priority) ; - /** - * Unsubscribe from a topic. - * - * @param handle A handle returned from orb_subscribe. - * @return OK on success, ERROR otherwise with errno set accordingly. - */ - int orb_unsubscribe(int handle) ; + /** + * Set the minimum interval between which updates are seen for a subscription. + * + * If this interval is set, the subscriber will not see more than one update + * within the period. + * + * Specifically, the first time an update is reported to the subscriber a timer + * is started. The update will continue to be reported via poll and orb_check, but + * once fetched via orb_copy another update will not be reported until the timer + * expires. + * + * This feature can be used to pace a subscriber that is watching a topic that + * would otherwise update too quickly. + * + * @param handle A handle returned from orb_subscribe. + * @param interval An interval period in milliseconds. + * @return OK on success, ERROR otherwise with ERRNO set accordingly. + */ + int orb_set_interval(int handle, unsigned interval) ; - /** - * Fetch data from a topic. - * - * This is the only operation that will reset the internal marker that - * indicates that a topic has been updated for a subscriber. Once poll - * or check return indicating that an updaet is available, this call - * must be used to update the subscription. - * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @param handle A handle returned from orb_subscribe. - * @param buffer Pointer to the buffer receiving the data, or NULL - * if the caller wants to clear the updated flag without - * using the data. - * @return OK on success, ERROR otherwise with errno set accordingly. - */ - int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) ; + /** + * Method to set the uORBCommunicator::IChannel instance. + * @param comm_channel + * The IChannel instance to talk to remote proxies. + * @note: + * Currently this call only supports the use of one IChannel + * Future extensions may include more than one IChannel's. + */ + void set_uorb_communicator(uORBCommunicator::IChannel *comm_channel); - /** - * Check whether a topic has been published to since the last orb_copy. - * - * This check can be used to determine whether to copy the topic when - * not using poll(), or to avoid the overhead of calling poll() when the - * topic is likely to have updated. - * - * Updates are tracked on a per-handle basis; this call will continue to - * return true until orb_copy is called using the same handle. This interface - * should be preferred over calling orb_stat due to the race window between - * stat and copy that can lead to missed updates. - * - * @param handle A handle returned from orb_subscribe. - * @param updated Set to true if the topic has been updated since the - * last time it was copied using this handle. - * @return OK if the check was successful, ERROR otherwise with - * errno set accordingly. - */ - int orb_check(int handle, bool *updated) ; + /** + * Gets the uORB Communicator instance. + */ + uORBCommunicator::IChannel *get_uorb_communicator(void); - /** - * Return the last time that the topic was updated. - * - * @param handle A handle returned from orb_subscribe. - * @param time Returns the absolute time that the topic was updated, or zero if it has - * never been updated. Time is measured in microseconds. - * @return OK on success, ERROR otherwise with errno set accordingly. - */ - int orb_stat(int handle, uint64_t *time) ; + /** + * Utility method to check if there is a remote subscriber present + * for a given topic + */ + bool is_remote_subscriber_present(const char *messageName); - /** - * Check if a topic has already been created. - * - * @param meta ORB topic metadata. - * @param instance ORB instance - * @return OK if the topic exists, ERROR otherwise with errno set accordingly. - */ - int orb_exists(const struct orb_metadata *meta, int instance) ; +private: // class methods + /** + * Advertise a node; don't consider it an error if the node has + * already been advertised. + * + * @todo verify that the existing node is the same as the one + * we tried to advertise. + */ + int + node_advertise + ( + const struct orb_metadata *meta, + int *instance = nullptr, + int priority = ORB_PRIO_DEFAULT + ); - /** - * Return the priority of the topic - * - * @param handle A handle returned from orb_subscribe. - * @param priority Returns the priority of this topic. This is only relevant for - * topics which are published by multiple publishers (e.g. mag0, mag1, etc.) - * and allows a subscriber to automatically pick the topic with the highest - * priority, independent of the startup order of the associated publishers. - * @return OK on success, ERROR otherwise with errno set accordingly. - */ - int orb_priority(int handle, int *priority) ; + /** + * Common implementation for orb_advertise and orb_subscribe. + * + * Handles creation of the object and the initial publication for + * advertisers. + */ + int + node_open + ( + Flavor f, + const struct orb_metadata *meta, + const void *data, + bool advertiser, + int *instance = nullptr, + int priority = ORB_PRIO_DEFAULT + ); - /** - * Set the minimum interval between which updates are seen for a subscription. - * - * If this interval is set, the subscriber will not see more than one update - * within the period. - * - * Specifically, the first time an update is reported to the subscriber a timer - * is started. The update will continue to be reported via poll and orb_check, but - * once fetched via orb_copy another update will not be reported until the timer - * expires. - * - * This feature can be used to pace a subscriber that is watching a topic that - * would otherwise update too quickly. - * - * @param handle A handle returned from orb_subscribe. - * @param interval An interval period in milliseconds. - * @return OK on success, ERROR otherwise with ERRNO set accordingly. - */ - int orb_set_interval(int handle, unsigned interval) ; +private: // data members + static Manager *_Instance; + // the communicator channel instance. + uORBCommunicator::IChannel *_comm_channel; + ORBSet _remote_subscriber_topics; - private: // class methods - /** - * Advertise a node; don't consider it an error if the node has - * already been advertised. - * - * @todo verify that the existing node is the same as the one - * we tried to advertise. - */ - int - node_advertise - ( - const struct orb_metadata *meta, - int *instance = nullptr, - int priority = ORB_PRIO_DEFAULT - ); +private: //class methods + Manager(); - /** - * Common implementation for orb_advertise and orb_subscribe. - * - * Handles creation of the object and the initial publication for - * advertisers. - */ - int - node_open - ( - Flavor f, - const struct orb_metadata *meta, - const void *data, - bool advertiser, - int *instance = nullptr, - int priority = ORB_PRIO_DEFAULT - ); + /** + * Interface to process a received AddSubscription from remote. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_add_subscription(const char *messageName, + int32_t msgRateInHz); - private: // data members - static Manager _Instance; + /** + * Interface to process a received control msg to remove subscription + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_remove_subscription(const char *messageName); - private: //class methods - Manager(); + /** + * Interface to process the received data message. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_received_message(const char *messageName, + int32_t length, uint8_t *data); }; diff --git a/src/modules/uORB/uORBManager_nuttx.cpp b/src/modules/uORB/uORBManager_nuttx.cpp index e8784d593eb1..3c4d88602317 100644 --- a/src/modules/uORB/uORBManager_nuttx.cpp +++ b/src/modules/uORB/uORBManager_nuttx.cpp @@ -39,244 +39,371 @@ #include #include "uORBUtils.hpp" #include "uORBManager.hpp" -#include "uORBDevices.hpp" //========================= Static initializations ================= -uORB::Manager uORB::Manager::_Instance; +uORB::Manager *uORB::Manager::_Instance = nullptr; //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -uORB::Manager* uORB::Manager::get_instance() +uORB::Manager *uORB::Manager::get_instance() { - return &_Instance; + if (_Instance == nullptr) { + _Instance = new uORB::Manager(); + } + + return _Instance; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- uORB::Manager::Manager() + : _comm_channel(nullptr) { } int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) { - /* - * Generate the path to the node and try to open it. - */ - char path[orb_maxpath]; - int inst = instance; - int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst); - - if (ret != OK) { - errno = -ret; - return uORB::ERROR; - } - - struct stat buffer; - return stat(path, &buffer); + /* + * Generate the path to the node and try to open it. + */ + char path[orb_maxpath]; + int inst = instance; + int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst); + + if (ret != OK) { + errno = -ret; + return uORB::ERROR; + } + + struct stat buffer; + + return stat(path, &buffer); } orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data) { - return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); + return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); } -orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) +orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, + int priority) { - int result, fd; - orb_advert_t advertiser; - - /* open the node as an advertiser */ - fd = node_open(PUBSUB, meta, data, true, instance, priority); - if (fd == ERROR) - return nullptr; - - /* get the advertiser handle and close the node */ - result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); - close(fd); - if (result == ERROR) - return nullptr; - - /* the advertiser must perform an initial publish to initialise the object */ - result = orb_publish(meta, advertiser, data); - if (result == ERROR) - return nullptr; - - return advertiser; + int result, fd; + orb_advert_t advertiser; + + /* open the node as an advertiser */ + fd = node_open(PUBSUB, meta, data, true, instance, priority); + + if (fd == ERROR) { + return nullptr; + } + + /* get the advertiser handle and close the node */ + result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + close(fd); + + if (result == ERROR) { + return nullptr; + } + + /* the advertiser must perform an initial publish to initialise the object */ + result = orb_publish(meta, advertiser, data); + + if (result == ERROR) { + return nullptr; + } + + return advertiser; } int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) { - return node_open(PUBSUB, meta, nullptr, false); + return node_open(PUBSUB, meta, nullptr, false); } int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) { - int inst = instance; - return node_open(PUBSUB, meta, nullptr, false, &inst); + int inst = instance; + return node_open(PUBSUB, meta, nullptr, false, &inst); } int uORB::Manager::orb_unsubscribe(int handle) { - return close(handle); + return close(handle); } int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { - return uORB::DeviceNode::publish(meta, handle, data); + return uORB::DeviceNode::publish(meta, handle, data); } int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) { - int ret; + int ret; - ret = read(handle, buffer, meta->o_size); + ret = read(handle, buffer, meta->o_size); - if (ret < 0) - return ERROR; + if (ret < 0) { + return ERROR; + } - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } - return OK; + return OK; } int uORB::Manager::orb_check(int handle, bool *updated) { - return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); + return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); } int uORB::Manager::orb_stat(int handle, uint64_t *time) { - return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); + return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); } -int uORB::Manager::orb_priority(int handle, int *priority) +int uORB::Manager::orb_priority(int handle, int32_t *priority) { - return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); + return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); } int uORB::Manager::orb_set_interval(int handle, unsigned interval) { - return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); + return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); } int uORB::Manager::node_advertise ( - const struct orb_metadata *meta, - int *instance, - int priority + const struct orb_metadata *meta, + int *instance, + int priority ) { - int fd = -1; - int ret = ERROR; + int fd = -1; + int ret = ERROR; - /* fill advertiser data */ - const struct orb_advertdata adv = { meta, instance, priority }; + /* fill advertiser data */ + const struct orb_advertdata adv = { meta, instance, priority }; - /* open the control device */ - fd = open(TOPIC_MASTER_DEVICE_PATH, 0); + /* open the control device */ + fd = open(TOPIC_MASTER_DEVICE_PATH, 0); - if (fd < 0) - goto out; + if (fd < 0) { + goto out; + } - /* advertise the object */ - ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); + /* advertise the object */ + ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); - /* it's OK if it already exists */ - if ((OK != ret) && (EEXIST == errno)) { - ret = OK; - } + /* it's OK if it already exists */ + if ((OK != ret) && (EEXIST == errno)) { + ret = OK; + } out: - if (fd >= 0) - close(fd); + if (fd >= 0) { + close(fd); + } - return ret; + return ret; } int uORB::Manager::node_open ( - Flavor f, - const struct orb_metadata *meta, - const void *data, - bool advertiser, - int *instance, - int priority + Flavor f, + const struct orb_metadata *meta, + const void *data, + bool advertiser, + int *instance, + int priority ) { - char path[orb_maxpath]; - int fd, ret; - - /* - * If meta is null, the object was not defined, i.e. it is not - * known to the system. We can't advertise/subscribe such a thing. - */ - if (nullptr == meta) { - errno = ENOENT; - return ERROR; - } - - /* - * Advertiser must publish an initial value. - */ - if (advertiser && (data == nullptr)) { - errno = EINVAL; - return ERROR; - } - - /* - * Generate the path to the node and try to open it. - */ - ret = uORB::Utils::node_mkpath(path, f, meta, instance); - - if (ret != OK) { - errno = -ret; - return ERROR; - } - - /* open the path as either the advertiser or the subscriber */ - fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); - - /* if we want to advertise and the node existed, we have to re-try again */ - if ((fd >= 0) && (instance != nullptr) && (advertiser)) { - /* close the fd, we want a new one */ - close(fd); - /* the node_advertise call will automatically go for the next free entry */ - fd = -1; - } - - /* we may need to advertise the node... */ - if (fd < 0) { - - /* try to create the node */ - ret = node_advertise(meta, instance, priority); - - if (ret == OK) { - /* update the path, as it might have been updated during the node_advertise call */ - ret = uORB::Utils::node_mkpath(path, f, meta, instance); - - if (ret != OK) { - errno = -ret; - return ERROR; - } - } - - /* on success, try the open again */ - if (ret == OK) { - fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); - } - } - - if (fd < 0) { - errno = EIO; - return ERROR; - } - - /* everything has been OK, we can return the handle now */ - return fd; + char path[orb_maxpath]; + int fd, ret; + + /* + * If meta is null, the object was not defined, i.e. it is not + * known to the system. We can't advertise/subscribe such a thing. + */ + if (nullptr == meta) { + errno = ENOENT; + return ERROR; + } + + /* + * Advertiser must publish an initial value. + */ + if (advertiser && (data == nullptr)) { + errno = EINVAL; + return ERROR; + } + + /* + * Generate the path to the node and try to open it. + */ + ret = uORB::Utils::node_mkpath(path, f, meta, instance); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + + /* open the path as either the advertiser or the subscriber */ + fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + + /* if we want to advertise and the node existed, we have to re-try again */ + if ((fd >= 0) && (instance != nullptr) && (advertiser)) { + /* close the fd, we want a new one */ + close(fd); + /* the node_advertise call will automatically go for the next free entry */ + fd = -1; + } + + /* we may need to advertise the node... */ + if (fd < 0) { + + /* try to create the node */ + ret = node_advertise(meta, instance, priority); + + if (ret == OK) { + /* update the path, as it might have been updated during the node_advertise call */ + ret = uORB::Utils::node_mkpath(path, f, meta, instance); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + } + + /* on success, try the open again */ + if (ret == OK) { + fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + } + } + + if (fd < 0) { + errno = EIO; + return ERROR; + } + + /* everything has been OK, we can return the handle now */ + return fd; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel) +{ + _comm_channel = channel; + + if (_comm_channel != nullptr) { + _comm_channel->register_handler(this); + } +} + +uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator(void) +{ + return _comm_channel; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_add_subscription(const char *messageName, + int32_t msgRateInHz) +{ + warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", + __LINE__, messageName); + int16_t rc = 0; + _remote_subscriber_topics.insert(messageName); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName); + + if (ret == OK) { + // get the node name. + uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath); + + if (node == nullptr) { + warnx("[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", + __LINE__, messageName); + + } else { + // node is present. + node->process_add_subscription(msgRateInHz); + } + + } else { + rc = -1; + } + + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_remove_subscription( + const char *messageName) +{ + warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", + __LINE__, messageName); + int16_t rc = -1; + _remote_subscriber_topics.erase(messageName); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName); + + if (ret == OK) { + uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath); + + // get the node name. + if (node == nullptr) { + warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", + __LINE__, messageName); + + } else { + // node is present. + node->process_remove_subscription(); + rc = 0; + } + } + + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_received_message(const char *messageName, + int32_t length, uint8_t *data) +{ + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName ); + + int16_t rc = -1; + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName); + + if (ret == OK) { + uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath); + + // get the node name. + if (node == nullptr) { + warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", + __LINE__, messageName, nodepath); + + } else { + // node is present. + node->process_received_message(length, data); + rc = 0; + } + } + + return rc; +} + +bool uORB::Manager::is_remote_subscriber_present(const char *messageName) +{ + return _remote_subscriber_topics.find(messageName); } diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index 7f70f348f3ec..7fe3f6a687b2 100644 --- a/src/modules/uORB/uORBManager_posix.cpp +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -32,265 +32,388 @@ ****************************************************************************/ #include -#include #include -#include #include #include +#include +#include #include "uORBUtils.hpp" #include "uORBManager.hpp" -#include "uORBDevices.hpp" #include "px4_config.h" +#include "uORBDevices.hpp" //========================= Static initializations ================= -uORB::Manager uORB::Manager::_Instance; +uORB::Manager *uORB::Manager::_Instance = nullptr; //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -uORB::Manager* uORB::Manager::get_instance() +uORB::Manager *uORB::Manager::get_instance() { - return &_Instance; + if (_Instance == nullptr) { + _Instance = new uORB::Manager(); + } + + return _Instance; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- uORB::Manager::Manager() + : _comm_channel(nullptr) { } int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) { - /* - * Generate the path to the node and try to open it. - */ - char path[orb_maxpath]; - int inst = instance; - int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst); - - if (ret != OK) { - errno = -ret; - return ERROR; - } - - struct stat buffer; - return stat(path, &buffer); + /* + * Generate the path to the node and try to open it. + */ + char path[orb_maxpath]; + int inst = instance; + int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + + return px4_access(path, F_OK); } orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data) { - //warnx("orb_advertise meta = %p", meta); - return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); + //warnx("orb_advertise meta = %p", meta); + return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); } -orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) +orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, + int priority) { - int result, fd; - orb_advert_t advertiser; - - //warnx("orb_advertise_multi meta = %p\n", meta); - - /* open the node as an advertiser */ - fd = node_open(PUBSUB, meta, data, true, instance, priority); - if (fd == ERROR) { - warnx("node_open as advertiser failed."); - return nullptr; - } - - /* get the advertiser handle and close the node */ - result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); - px4_close(fd); - if (result == ERROR) { - warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); - return nullptr; - } - - /* the advertiser must perform an initial publish to initialise the object */ - result = orb_publish(meta, advertiser, data); - if (result == ERROR) { - warnx("orb_publish failed"); - return nullptr; - } - - return advertiser; + int result, fd; + orb_advert_t advertiser; + + //warnx("orb_advertise_multi meta = %p\n", meta); + + /* open the node as an advertiser */ + fd = node_open(PUBSUB, meta, data, true, instance, priority); + + if (fd == ERROR) { + warnx("node_open as advertiser failed."); + return nullptr; + } + + /* get the advertiser handle and close the node */ + result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + px4_close(fd); + + if (result == ERROR) { + warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); + return nullptr; + } + + /* the advertiser must perform an initial publish to initialise the object */ + result = orb_publish(meta, advertiser, data); + + if (result == ERROR) { + warnx("orb_publish failed"); + return nullptr; + } + + return advertiser; } int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) { - return node_open(PUBSUB, meta, nullptr, false); + return node_open(PUBSUB, meta, nullptr, false); } int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) { - int inst = instance; - return node_open(PUBSUB, meta, nullptr, false, &inst); + int inst = instance; + return node_open(PUBSUB, meta, nullptr, false, &inst); } int uORB::Manager::orb_unsubscribe(int fd) { - return px4_close(fd); + return px4_close(fd); } int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { - return uORB::DeviceNode::publish(meta, handle, data); + return uORB::DeviceNode::publish(meta, handle, data); } int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) { - int ret; + int ret; - ret = px4_read(handle, buffer, meta->o_size); + ret = px4_read(handle, buffer, meta->o_size); - if (ret < 0) - return ERROR; + if (ret < 0) { + return ERROR; + } - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } - return PX4_OK; + return PX4_OK; } int uORB::Manager::orb_check(int handle, bool *updated) { - return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); + return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); } int uORB::Manager::orb_stat(int handle, uint64_t *time) { - return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); + return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); } -int uORB::Manager::orb_priority(int handle, int *priority) +int uORB::Manager::orb_priority(int handle, int32_t *priority) { - return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); + return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); } int uORB::Manager::orb_set_interval(int handle, unsigned interval) { - return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); + return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); } int uORB::Manager::node_advertise ( - const struct orb_metadata *meta, - int *instance, - int priority + const struct orb_metadata *meta, + int *instance, + int priority ) { - int fd = -1; - int ret = ERROR; + int fd = -1; + int ret = ERROR; - /* fill advertiser data */ - const struct orb_advertdata adv = { meta, instance, priority }; + /* fill advertiser data */ + const struct orb_advertdata adv = { meta, instance, priority }; - /* open the control device */ - fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0); + /* open the control device */ + fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0); - if (fd < 0) - goto out; + if (fd < 0) { + goto out; + } - /* advertise the object */ - ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); + /* advertise the object */ + ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); - /* it's PX4_OK if it already exists */ - if ((PX4_OK != ret) && (EEXIST == errno)) { - ret = PX4_OK; - } + /* it's PX4_OK if it already exists */ + if ((PX4_OK != ret) && (EEXIST == errno)) { + ret = PX4_OK; + } out: - if (fd >= 0) - px4_close(fd); + if (fd >= 0) { + px4_close(fd); + } - return ret; + return ret; } int uORB::Manager::node_open ( - Flavor f, - const struct orb_metadata *meta, - const void *data, - bool advertiser, - int *instance, - int priority + Flavor f, + const struct orb_metadata *meta, + const void *data, + bool advertiser, + int *instance, + int priority ) { - char path[orb_maxpath]; - int fd, ret; - - /* - * If meta is null, the object was not defined, i.e. it is not - * known to the system. We can't advertise/subscribe such a thing. - */ - if (nullptr == meta) { - errno = ENOENT; - return ERROR; - } - - /* - * Advertiser must publish an initial value. - */ - if (advertiser && (data == nullptr)) { - errno = EINVAL; - return ERROR; - } - - /* - * Generate the path to the node and try to open it. - */ - - // FIXME - if *instance is uninitialized, why is this being called? Seems risky and - // its definiately a waste. This is the case in muli-topic test. - ret = uORB::Utils::node_mkpath(path, f, meta, instance); - - if (ret != PX4_OK) { - errno = -ret; - return ERROR; - } - - /* open the path as either the advertiser or the subscriber */ - fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); - - /* if we want to advertise and the node existed, we have to re-try again */ - if ((fd >= 0) && (instance != nullptr) && (advertiser)) { - /* close the fd, we want a new one */ - px4_close(fd); - - /* the node_advertise call will automatically go for the next free entry */ - fd = -1; - } - - /* we may need to advertise the node... */ - if (fd < 0) { - - /* try to create the node */ - ret = node_advertise(meta, instance, priority); - - if (ret == PX4_OK) { - /* update the path, as it might have been updated during the node_advertise call */ - ret = uORB::Utils::node_mkpath(path, f, meta, instance); - - if (ret != PX4_OK) { - errno = -ret; - return ERROR; - } - } - - /* on success, try the open again */ - if (ret == PX4_OK) { - fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); - } - } - - if (fd < 0) { - errno = EIO; - return ERROR; - } - - /* everything has been OK, we can return the handle now */ - return fd; + char path[orb_maxpath]; + int fd, ret; + + /* + * If meta is null, the object was not defined, i.e. it is not + * known to the system. We can't advertise/subscribe such a thing. + */ + if (nullptr == meta) { + errno = ENOENT; + return ERROR; + } + + /* + * Advertiser must publish an initial value. + */ + if (advertiser && (data == nullptr)) { + errno = EINVAL; + return ERROR; + } + + /* + * Generate the path to the node and try to open it. + */ + + // FIXME - if *instance is uninitialized, why is this being called? Seems risky and + // its definiately a waste. This is the case in muli-topic test. + ret = uORB::Utils::node_mkpath(path, f, meta, instance); + + if (ret != PX4_OK) { + errno = -ret; + return ERROR; + } + + /* open the path as either the advertiser or the subscriber */ + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + + /* if we want to advertise and the node existed, we have to re-try again */ + if ((fd >= 0) && (instance != nullptr) && (advertiser)) { + /* close the fd, we want a new one */ + px4_close(fd); + + /* the node_advertise call will automatically go for the next free entry */ + fd = -1; + } + + /* we may need to advertise the node... */ + if (fd < 0) { + + /* try to create the node */ + ret = node_advertise(meta, instance, priority); + + if (ret == PX4_OK) { + /* update the path, as it might have been updated during the node_advertise call */ + ret = uORB::Utils::node_mkpath(path, f, meta, instance); + + if (ret != PX4_OK) { + errno = -ret; + return ERROR; + } + } + + /* on success, try the open again */ + if (ret == PX4_OK) { + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + } + } + + if (fd < 0) { + errno = EIO; + return ERROR; + } + + /* everything has been OK, we can return the handle now */ + return fd; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel) +{ + _comm_channel = channel; + + if (_comm_channel != nullptr) { + _comm_channel->register_handler(this); + } +} + +uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator(void) +{ + return _comm_channel; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_add_subscription(const char *messageName, + int32_t msgRateInHz) +{ + warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", + __LINE__, messageName); + int16_t rc = 0; + _remote_subscriber_topics.insert(messageName); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName); + + if (ret == OK) { + // get the node name. + uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath); + + if (node == nullptr) { + warnx("[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", + __LINE__, messageName); + + } else { + // node is present. + node->process_add_subscription(msgRateInHz); + } + + } else { + rc = -1; + } + + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_remove_subscription( + const char *messageName) +{ + warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", + __LINE__, messageName); + int16_t rc = -1; + _remote_subscriber_topics.erase(messageName); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName); + + if (ret == OK) { + uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath); + + // get the node name. + if (node == nullptr) { + warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", + __LINE__, messageName); + + } else { + // node is present. + node->process_remove_subscription(); + rc = 0; + } + } + + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_received_message(const char *messageName, + int32_t length, uint8_t *data) +{ + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName ); + + int16_t rc = -1; + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName); + + if (ret == OK) { + uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath); + + // get the node name. + if (node == nullptr) { + warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", + __LINE__, messageName, nodepath); + + } else { + // node is present. + node->process_received_message(length, data); + rc = 0; + } + } + + return rc; +} + +bool uORB::Manager::is_remote_subscriber_present(const char *messageName) +{ + return (_remote_subscriber_topics.find(messageName) != _remote_subscriber_topics.end()); } diff --git a/src/modules/uORB/uORBTest_UnitTest.cpp b/src/modules/uORB/uORBTest_UnitTest.cpp index 53a053dc9218..6b30e75b2f33 100644 --- a/src/modules/uORB/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORBTest_UnitTest.cpp @@ -45,246 +45,381 @@ uORBTest::UnitTest &uORBTest::UnitTest::instance() int uORBTest::UnitTest::pubsublatency_main(void) { - /* poll on test topic and output latency */ - float latency_integral = 0.0f; - - /* wakeup source(s) */ - px4_pollfd_struct_t fds[3]; - - int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); - int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); - int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0); - - struct orb_test_large t; - - /* clear all ready flags */ - orb_copy(ORB_ID(orb_test), test_multi_sub, &t); - orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); - orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); - - fds[0].fd = test_multi_sub; - fds[0].events = POLLIN; - fds[1].fd = test_multi_sub_medium; - fds[1].events = POLLIN; - fds[2].fd = test_multi_sub_large; - fds[2].events = POLLIN; - - const unsigned maxruns = 1000; - unsigned timingsgroup = 0; - - unsigned *timings = new unsigned[maxruns]; - - for (unsigned i = 0; i < maxruns; i++) { - /* wait for up to 500ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(orb_test), test_multi_sub, &t); - timingsgroup = 0; - } else if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); - timingsgroup = 1; - } else if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); - timingsgroup = 2; - } - - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } - - hrt_abstime elt = hrt_elapsed_time(&t.time); - latency_integral += elt; - timings[i] = elt; - } - - orb_unsubscribe(test_multi_sub); - orb_unsubscribe(test_multi_sub_medium); - orb_unsubscribe(test_multi_sub_large); - - if (pubsubtest_print) { - char fname[32]; - //sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup); - sprintf(fname, "/tmp/timings%u.txt", timingsgroup); - FILE *f = fopen(fname, "w"); - if (f == NULL) { - warnx("Error opening file!\n"); - return uORB::ERROR; - } - - for (unsigned i = 0; i < maxruns; i++) { - fprintf(f, "%u\n", timings[i]); - } - - fclose(f); - } - - delete[] timings; - - warnx("mean: %8.4f", static_cast(latency_integral / maxruns)); - - pubsubtest_passed = true; - - if (static_cast(latency_integral / maxruns) > 30.0f) { - pubsubtest_res = uORB::ERROR; - } else { - pubsubtest_res = PX4_OK; - } - - return pubsubtest_res; + /* poll on test topic and output latency */ + float latency_integral = 0.0f; + + /* wakeup source(s) */ + px4_pollfd_struct_t fds[3]; + + int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); + int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); + int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0); + + struct orb_test_large t; + + /* clear all ready flags */ + orb_copy(ORB_ID(orb_test), test_multi_sub, &t); + orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); + orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); + + fds[0].fd = test_multi_sub; + fds[0].events = POLLIN; + fds[1].fd = test_multi_sub_medium; + fds[1].events = POLLIN; + fds[2].fd = test_multi_sub_large; + fds[2].events = POLLIN; + + const unsigned maxruns = 1000; + unsigned timingsgroup = 0; + + unsigned *timings = new unsigned[maxruns]; + + for (unsigned i = 0; i < maxruns; i++) { + /* wait for up to 500ms for data */ + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(orb_test), test_multi_sub, &t); + timingsgroup = 0; + + } else if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); + timingsgroup = 1; + + } else if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); + timingsgroup = 2; + } + + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + hrt_abstime elt = hrt_elapsed_time(&t.time); + latency_integral += elt; + timings[i] = elt; + } + + orb_unsubscribe(test_multi_sub); + orb_unsubscribe(test_multi_sub_medium); + orb_unsubscribe(test_multi_sub_large); + + if (pubsubtest_print) { + char fname[32]; + sprintf(fname, PX4_ROOTFSDIR"/fs/microsd/timings%u.txt", timingsgroup); + FILE *f = fopen(fname, "w"); + + if (f == NULL) { + warnx("Error opening file!\n"); + return uORB::ERROR; + } + + for (unsigned i = 0; i < maxruns; i++) { + fprintf(f, "%u\n", timings[i]); + } + + fclose(f); + } + + delete[] timings; + + warnx("mean: %8.4f", static_cast(latency_integral / maxruns)); + + pubsubtest_passed = true; + + if (static_cast(latency_integral / maxruns) > 30.0f) { + pubsubtest_res = uORB::ERROR; + + } else { + pubsubtest_res = PX4_OK; + } + + return pubsubtest_res; } int uORBTest::UnitTest::test() { - struct orb_test t, u; - int sfd; - orb_advert_t ptopic; - bool updated; + int ret = test_single(); + + if (ret != OK) { + return ret; + } + + ret = test_multi(); + + if (ret != OK) { + return ret; + } + + ret = test_multi_reversed(); + + if (ret != OK) { + return ret; + } + + return OK; +} + + +int uORBTest::UnitTest::info() +{ + return OK; +} + +int uORBTest::UnitTest::test_single() +{ + test_note("try single-topic support"); + + struct orb_test t, u; + int sfd; + orb_advert_t ptopic; + bool updated; + + t.val = 0; + ptopic = orb_advertise(ORB_ID(orb_test), &t); - t.val = 0; - ptopic = orb_advertise(ORB_ID(orb_test), &t); + if (ptopic == nullptr) { + return test_fail("advertise failed: %d", errno); + } - if (ptopic == nullptr) - return test_fail("advertise failed: %d", errno); + test_note("publish handle 0x%08x", ptopic); + sfd = orb_subscribe(ORB_ID(orb_test)); - test_note("publish handle 0x%08x", ptopic); - sfd = orb_subscribe(ORB_ID(orb_test)); + if (sfd < 0) { + return test_fail("subscribe failed: %d", errno); + } - if (sfd < 0) - return test_fail("subscribe failed: %d", errno); + test_note("subscribe fd %d", sfd); + u.val = 1; - test_note("subscribe fd %d", sfd); - u.val = 1; + if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) { + return test_fail("copy(1) failed: %d", errno); + } - if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) - return test_fail("copy(1) failed: %d", errno); + if (u.val != t.val) { + return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); + } - if (u.val != t.val) - return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); + if (PX4_OK != orb_check(sfd, &updated)) { + return test_fail("check(1) failed"); + } - if (PX4_OK != orb_check(sfd, &updated)) - return test_fail("check(1) failed"); + if (updated) { + return test_fail("spurious updated flag"); + } - if (updated) - return test_fail("spurious updated flag"); + t.val = 2; + test_note("try publish"); - t.val = 2; - test_note("try publish"); + if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t)) { + return test_fail("publish failed"); + } - if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t)) - return test_fail("publish failed"); + if (PX4_OK != orb_check(sfd, &updated)) { + return test_fail("check(2) failed"); + } - if (PX4_OK != orb_check(sfd, &updated)) - return test_fail("check(2) failed"); + if (!updated) { + return test_fail("missing updated flag"); + } - if (!updated) - return test_fail("missing updated flag"); + if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) { + return test_fail("copy(2) failed: %d", errno); + } + + if (u.val != t.val) { + return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); + } + + orb_unsubscribe(sfd); + + return test_note("PASS single-topic test"); +} - if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) - return test_fail("copy(2) failed: %d", errno); +int uORBTest::UnitTest::test_multi() +{ + /* this routine tests the multi-topic support */ + test_note("try multi-topic support"); - if (u.val != t.val) - return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); + struct orb_test t, u; + t.val = 0; + int instance0; + orb_advert_t pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); - orb_unsubscribe(sfd); + test_note("advertised"); - /* this routine tests the multi-topic support */ - test_note("try multi-topic support"); + int instance1; + orb_advert_t pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); - int instance0; - orb_advert_t pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); + if (instance0 != 0) { + return test_fail("mult. id0: %d", instance0); + } - test_note("advertised"); + if (instance1 != 1) { + return test_fail("mult. id1: %d", instance1); + } - int instance1; - orb_advert_t pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); + t.val = 103; - if (instance0 != 0) - return test_fail("mult. id0: %d", instance0); + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) { + return test_fail("mult. pub0 fail"); + } - if (instance1 != 1) - return test_fail("mult. id1: %d", instance1); + test_note("published"); - t.val = 103; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) - return test_fail("mult. pub0 fail"); + t.val = 203; - test_note("published"); + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) { + return test_fail("mult. pub1 fail"); + } - t.val = 203; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) - return test_fail("mult. pub1 fail"); + /* subscribe to both topics and ensure valid data is received */ + int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); - /* subscribe to both topics and ensure valid data is received */ - int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); + if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) { + return test_fail("sub #0 copy failed: %d", errno); + } - if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) - return test_fail("sub #0 copy failed: %d", errno); + if (u.val != 103) { + return test_fail("sub #0 val. mismatch: %d", u.val); + } - if (u.val != 103) - return test_fail("sub #0 val. mismatch: %d", u.val); + int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); - int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); + if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) { + return test_fail("sub #1 copy failed: %d", errno); + } - if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) - return test_fail("sub #1 copy failed: %d", errno); + if (u.val != 203) { + return test_fail("sub #1 val. mismatch: %d", u.val); + } - if (u.val != 203) - return test_fail("sub #1 val. mismatch: %d", u.val); + /* test priorities */ + int prio; - /* test priorities */ - int prio; - if (PX4_OK != orb_priority(sfd0, &prio)) - return test_fail("prio #0"); + if (PX4_OK != orb_priority(sfd0, &prio)) { + return test_fail("prio #0"); + } - if (prio != ORB_PRIO_MAX) - return test_fail("prio: %d", prio); + if (prio != ORB_PRIO_MAX) { + return test_fail("prio: %d", prio); + } - if (PX4_OK != orb_priority(sfd1, &prio)) - return test_fail("prio #1"); + if (PX4_OK != orb_priority(sfd1, &prio)) { + return test_fail("prio #1"); + } - if (prio != ORB_PRIO_MIN) - return test_fail("prio: %d", prio); + if (prio != ORB_PRIO_MIN) { + return test_fail("prio: %d", prio); + } - if (PX4_OK != latency_test(ORB_ID(orb_test), false)) - return test_fail("latency test failed"); + if (PX4_OK != latency_test(ORB_ID(orb_test), false)) { + return test_fail("latency test failed"); + } - return test_note("PASS"); + return test_note("PASS multi-topic test"); } -int uORBTest::UnitTest::info() +int uORBTest::UnitTest::test_multi_reversed() { - return OK; + test_note("try multi-topic support subscribing before publishing"); + + /* For these tests 0 and 1 instances are taken from before, therefore continue with 2 and 3. */ + + /* Subscribe first and advertise afterwards. */ + int sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2); + + if (sfd2 < 0) { + return test_fail("sub. id2: ret: %d", sfd2); + } + + struct orb_test t, u; + + t.val = 0; + + int instance2; + + orb_advert_t pfd2 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2, ORB_PRIO_MAX); + + int instance3; + + orb_advert_t pfd3 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3, ORB_PRIO_MIN); + + test_note("advertised"); + + if (instance2 != 2) { + return test_fail("mult. id2: %d", instance2); + } + + if (instance3 != 3) { + return test_fail("mult. id3: %d", instance3); + } + + t.val = 204; + + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd2, &t)) { + return test_fail("mult. pub0 fail"); + } + + + t.val = 304; + + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd3, &t)) { + return test_fail("mult. pub1 fail"); + } + + test_note("published"); + + if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd2, &u)) { + return test_fail("sub #2 copy failed: %d", errno); + } + + if (u.val != 204) { + return test_fail("sub #3 val. mismatch: %d", u.val); + } + + int sfd3 = orb_subscribe_multi(ORB_ID(orb_multitest), 3); + + if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd3, &u)) { + return test_fail("sub #3 copy failed: %d", errno); + } + + if (u.val != 304) { + return test_fail("sub #3 val. mismatch: %d", u.val); + } + + return test_note("PASS multi-topic reversed"); } int uORBTest::UnitTest::test_fail(const char *fmt, ...) { - va_list ap; - - fprintf(stderr, "FAIL: "); - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - return uORB::ERROR; + va_list ap; + + fprintf(stderr, "uORB FAIL: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return uORB::ERROR; } int uORBTest::UnitTest::test_note(const char *fmt, ...) { - va_list ap; - - fprintf(stderr, "note: "); - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - return OK; + va_list ap; + + fprintf(stderr, "uORB note: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return OK; } -int uORBTest::UnitTest::pubsubtest_threadEntry(char* const argv[]) +int uORBTest::UnitTest::pubsubtest_threadEntry(char *const argv[]) { - uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); - return t.pubsublatency_main(); + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); + return t.pubsublatency_main(); } diff --git a/src/modules/uORB/uORBTest_UnitTest.hpp b/src/modules/uORB/uORBTest_UnitTest.hpp index 93399f734f93..5f8d087ac1fa 100644 --- a/src/modules/uORB/uORBTest_UnitTest.hpp +++ b/src/modules/uORB/uORBTest_UnitTest.hpp @@ -38,101 +38,108 @@ #include struct orb_test { - int val; - hrt_abstime time; + int val; + hrt_abstime time; }; ORB_DEFINE(orb_test, struct orb_test); ORB_DEFINE(orb_multitest, struct orb_test); struct orb_test_medium { - int val; - hrt_abstime time; - char junk[64]; + int val; + hrt_abstime time; + char junk[64]; }; ORB_DEFINE(orb_test_medium, struct orb_test_medium); struct orb_test_large { - int val; - hrt_abstime time; - char junk[512]; + int val; + hrt_abstime time; + char junk[512]; }; ORB_DEFINE(orb_test_large, struct orb_test_large); namespace uORBTest { - class UnitTest; +class UnitTest; } class uORBTest::UnitTest { public: - // Singleton pattern - static uORBTest::UnitTest &instance(); - ~UnitTest() {} - int test(); - template int latency_test(orb_id_t T, bool print); - int info(); + // Singleton pattern + static uORBTest::UnitTest &instance(); + ~UnitTest() {} + int test(); + template int latency_test(orb_id_t T, bool print); + int info(); private: - UnitTest() : pubsubtest_passed(false), pubsubtest_print(false) {} - - // Disallow copy - UnitTest(const uORBTest::UnitTest &) {}; - static int pubsubtest_threadEntry(char* const argv[]); - int pubsublatency_main(void); - bool pubsubtest_passed; - bool pubsubtest_print; - int pubsubtest_res = OK; - - int test_fail(const char *fmt, ...); - int test_note(const char *fmt, ...); + UnitTest() : pubsubtest_passed(false), pubsubtest_print(false) {} + + // Disallow copy + UnitTest(const uORBTest::UnitTest &) {}; + static int pubsubtest_threadEntry(char *const argv[]); + int pubsublatency_main(void); + // + bool pubsubtest_passed; + bool pubsubtest_print; + int pubsubtest_res = OK; + + int test_single(); + int test_multi(); + int test_multi_reversed(); + + int test_fail(const char *fmt, ...); + int test_note(const char *fmt, ...); }; template int uORBTest::UnitTest::latency_test(orb_id_t T, bool print) { - test_note("---------------- LATENCY TEST ------------------"); - S t; - t.val = 308; - t.time = hrt_absolute_time(); - - orb_advert_t pfd0 = orb_advertise(T, &t); - - char * const args[1] = { NULL }; - - pubsubtest_print = print; - pubsubtest_passed = false; - - /* test pub / sub latency */ - - // Can't pass a pointer in args, must be a null terminated - // array of strings because the strings are copied to - // prevent access if the caller data goes out of scope - int pubsub_task = px4_task_spawn_cmd("uorb_latency", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - (px4_main_t)&uORBTest::UnitTest::pubsubtest_threadEntry, - args); - - /* give the test task some data */ - while (!pubsubtest_passed) { - t.val = 308; - t.time = hrt_absolute_time(); - if (PX4_OK != orb_publish(T, pfd0, &t)) - return test_fail("mult. pub0 timing fail"); - - /* simulate >800 Hz system operation */ - usleep(1000); - } - - if (pubsub_task < 0) { - return test_fail("failed launching task"); - } - - return pubsubtest_res; + test_note("---------------- LATENCY TEST ------------------"); + S t; + t.val = 308; + t.time = hrt_absolute_time(); + + orb_advert_t pfd0 = orb_advertise(T, &t); + + char *const args[1] = { NULL }; + + pubsubtest_print = print; + pubsubtest_passed = false; + + /* test pub / sub latency */ + + // Can't pass a pointer in args, must be a null terminated + // array of strings because the strings are copied to + // prevent access if the caller data goes out of scope + int pubsub_task = px4_task_spawn_cmd("uorb_latency", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (px4_main_t)&uORBTest::UnitTest::pubsubtest_threadEntry, + args); + + /* give the test task some data */ + while (!pubsubtest_passed) { + t.val = 308; + t.time = hrt_absolute_time(); + + if (PX4_OK != orb_publish(T, pfd0, &t)) { + return test_fail("mult. pub0 timing fail"); + } + + /* simulate >800 Hz system operation */ + usleep(1000); + } + + if (pubsub_task < 0) { + return test_fail("failed launching task"); + } + + return pubsubtest_res; } #endif // _uORBTest_UnitTest_hpp_ diff --git a/src/modules/uORB/uORBUtils.cpp b/src/modules/uORB/uORBUtils.cpp index c9d49222bc41..4b4e76c1705c 100644 --- a/src/modules/uORB/uORBUtils.cpp +++ b/src/modules/uORB/uORBUtils.cpp @@ -37,27 +37,46 @@ int uORB::Utils::node_mkpath ( - char *buf, - Flavor f, - const struct orb_metadata *meta, - int *instance + char *buf, + Flavor f, + const struct orb_metadata *meta, + int *instance ) { - unsigned len; + unsigned len; - unsigned index = 0; + unsigned index = 0; - if (instance != nullptr) { - index = *instance; - } + if (instance != nullptr) { + index = *instance; + } - len = snprintf(buf, orb_maxpath, "/%s/%s%d", - (f == PUBSUB) ? "obj" : "param", - meta->o_name, index); + len = snprintf(buf, orb_maxpath, "/%s/%s%d", + (f == PUBSUB) ? "obj" : "param", + meta->o_name, index); - if (len >= orb_maxpath) { - return -ENAMETOOLONG; - } + if (len >= orb_maxpath) { + return -ENAMETOOLONG; + } - return OK; + return OK; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int uORB::Utils::node_mkpath(char *buf, Flavor f, + const char *orbMsgName) +{ + unsigned len; + + unsigned index = 0; + + len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param", + orbMsgName, index); + + if (len >= orb_maxpath) { + return -ENAMETOOLONG; + } + + return OK; } diff --git a/src/modules/uORB/uORBUtils.hpp b/src/modules/uORB/uORBUtils.hpp index dc2fb9b78e05..4593f1a95e9b 100644 --- a/src/modules/uORB/uORBUtils.hpp +++ b/src/modules/uORB/uORBUtils.hpp @@ -34,22 +34,29 @@ #define _uORBUtils_hpp_ #include "uORBCommon.hpp" +#include namespace uORB { - class Utils; +class Utils; } class uORB::Utils { public: - static int node_mkpath - ( - char *buf, - Flavor f, - const struct orb_metadata *meta, - int *instance = nullptr - ); + static int node_mkpath + ( + char *buf, + Flavor f, + const struct orb_metadata *meta, + int *instance = nullptr + ); + + /** + * same as above except this generators the path based on the string. + */ + static int node_mkpath(char *buf, Flavor f, const char *orbMsgName); + }; #endif // _uORBUtils_hpp_ diff --git a/src/modules/unit_test/CMakeLists.txt b/src/modules/unit_test/CMakeLists.txt new file mode 100644 index 000000000000..6fd63d931db5 --- /dev/null +++ b/src/modules/unit_test/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__unit_test + COMPILE_FLAGS + -Os + SRCS + unit_test.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 8ed4efadf95c..bd021ff54a07 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -44,14 +44,14 @@ class __EXPORT UnitTest UnitTest(); virtual ~UnitTest(); - + /// @brief Override to run your unit tests. Unit tests should be called using ut_run_test macro. /// @return true: all unit tests succeeded, false: one or more unit tests failed virtual bool run_tests(void) = 0; - + /// @brief Prints results from running of unit tests. void print_results(void); - + /// @brief Macro to create a function which will run a unit test class and print results. #define ut_declare_test(test_function, test_class) \ bool test_function(void) \ @@ -61,9 +61,9 @@ class __EXPORT UnitTest test->print_results(); \ return success; \ } - + protected: - + /// @brief Runs a single unit test. Unit tests must have the function signature of bool test(void). The unit /// test should return true if it succeeded, false for fail. #define ut_run_test(test) \ @@ -80,7 +80,7 @@ class __EXPORT UnitTest } \ _cleanup(); \ } while (0) - + /// @brief Used to assert a value within a unit test. #define ut_assert(message, test) \ do { \ @@ -91,7 +91,7 @@ class __EXPORT UnitTest _assertions++; \ } \ } while (0) - + /// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert /// since it will give you better error reporting of the actual values being compared. #define ut_compare(message, v1, v2) \ @@ -105,12 +105,13 @@ class __EXPORT UnitTest _assertions++; \ } \ } while (0) - + virtual void _init(void) { }; ///< Run before each unit test. Override to provide custom behavior. virtual void _cleanup(void) { }; ///< Run after each unit test. Override to provide custom behavior. - - void _print_assert(const char* msg, const char* test, const char* file, int line); - void _print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); + + void _print_assert(const char *msg, const char *test, const char *file, int line); + void _print_compare(const char *msg, const char *v1_text, int v1, const char *v2_text, int v2, const char *file, + int line); int _tests_run; ///< The number of individual unit tests run int _tests_failed; ///< The number of unit tests which failed diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt new file mode 100644 index 000000000000..b7dcd83ed076 --- /dev/null +++ b/src/modules/vtol_att_control/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__vtol_att_control + MAIN vtol_att_control + COMPILE_FLAGS + -Wno-write-strings + + SRCS + vtol_att_control_main.cpp + tiltrotor.cpp + vtol_type.cpp + tailsitter.cpp + standard.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp new file mode 100644 index 000000000000..05c1a525b1ff --- /dev/null +++ b/src/modules/vtol_att_control/standard.cpp @@ -0,0 +1,325 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file standard.cpp + * + * @author Simon Wilks + * @author Roman Bapst + * +*/ + +#include "standard.h" +#include "vtol_att_control_main.h" + +Standard::Standard(VtolAttitudeControl *attc) : + VtolType(attc), + _flag_enable_mc_motors(true), + _pusher_throttle(0.0f), + _airspeed_trans_blend_margin(0.0f) +{ + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + _params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_standard.pusher_trans = param_find("VT_TRANS_THR"); + _params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND"); + _params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS"); +} + +Standard::~Standard() +{ +} + +int +Standard::parameters_update() +{ + float v; + + /* duration of a forwards transition to fw mode */ + param_get(_params_handles_standard.front_trans_dur, &v); + _params_standard.front_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* duration of a back transition to mc mode */ + param_get(_params_handles_standard.back_trans_dur, &v); + _params_standard.back_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* target throttle value for pusher motor during the transition to fw mode */ + param_get(_params_handles_standard.pusher_trans, &v); + _params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f); + + /* airspeed at which it we should switch to fw mode */ + param_get(_params_handles_standard.airspeed_trans, &v); + _params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f); + + /* airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_standard.airspeed_blend, &v); + _params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f); + + _airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend; + + return OK; +} + +void Standard::update_vtol_state() +{ + parameters_update(); + + /* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up + * forward speed. After the vehicle has picked up enough speed the rotors shutdown. + * For the back transition the pusher motor is immediately stopped and rotors reactivated. + */ + + if (!_attc->is_fixed_wing_requested()) { + // the transition to fw mode switch is off + if (_vtol_schedule.flight_mode == MC_MODE) { + // in mc mode + _vtol_schedule.flight_mode = MC_MODE; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + } else if (_vtol_schedule.flight_mode == FW_MODE) { + // transition to mc mode + _vtol_schedule.flight_mode = TRANSITION_TO_MC; + _flag_enable_mc_motors = true; + _vtol_schedule.transition_start = hrt_absolute_time(); + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + // failsafe back to mc mode + _vtol_schedule.flight_mode = MC_MODE; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // transition to MC mode if transition time has passed + if (hrt_elapsed_time(&_vtol_schedule.transition_start) > + (_params_standard.back_trans_dur * 1000000.0f)) { + _vtol_schedule.flight_mode = MC_MODE; + } + } + + // the pusher motor should never be powered when in or transitioning to mc mode + _pusher_throttle = 0.0f; + + } else { + // the transition to fw mode switch is on + if (_vtol_schedule.flight_mode == MC_MODE) { + // start transition to fw mode + _vtol_schedule.flight_mode = TRANSITION_TO_FW; + _vtol_schedule.transition_start = hrt_absolute_time(); + + } else if (_vtol_schedule.flight_mode == FW_MODE) { + // in fw mode + _vtol_schedule.flight_mode = FW_MODE; + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode + if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans || !_armed->armed) { + _vtol_schedule.flight_mode = FW_MODE; + // we can turn off the multirotor motors now + _flag_enable_mc_motors = false; + // don't set pusher throttle here as it's being ramped up elsewhere + } + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // transitioning to mc mode & transition switch on - failsafe back into fw mode + _vtol_schedule.flight_mode = FW_MODE; + } + } + + // map specific control phases to simple control modes + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + break; + + case FW_MODE: + _vtol_mode = FIXED_WING; + break; + + case TRANSITION_TO_FW: + case TRANSITION_TO_MC: + _vtol_mode = TRANSITION; + break; + } +} + +void Standard::update_transition_state() +{ + if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + if (_params_standard.front_trans_dur <= 0.0f) { + // just set the final target throttle value + _pusher_throttle = _params_standard.pusher_trans; + + } else if (_pusher_throttle <= _params_standard.pusher_trans) { + // ramp up throttle to the target throttle value + _pusher_throttle = _params_standard.pusher_trans * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f); + } + + // do blending of mc and fw controls if a blending airspeed has been provided + if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) { + float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / + _airspeed_trans_blend_margin; + _mc_roll_weight = weight; + _mc_pitch_weight = weight; + _mc_yaw_weight = weight; + + } else { + // at low speeds give full weight to mc + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + } + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // continually increase mc attitude control as we transition back to mc mode + if (_params_standard.back_trans_dur > 0.0f) { + float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * + 1000000.0f); + _mc_roll_weight = weight; + _mc_pitch_weight = weight; + _mc_yaw_weight = weight; + + } else { + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + } + + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + if (_flag_enable_mc_motors) { + set_max_mc(2000); + set_idle_mc(); + _flag_enable_mc_motors = false; + } + } + + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); +} + +void Standard::update_mc_state() +{ + // do nothing +} + +void Standard::update_fw_state() +{ + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + if (!_flag_enable_mc_motors) { + set_max_mc(950); + set_idle_fw(); // force them to stop, not just idle + _flag_enable_mc_motors = true; + } +} + +void Standard::update_external_state() +{ +} + +/** + * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine + * what proportion of control should be applied to each of the control groups (mc and fw). + */ +void Standard::fill_actuator_outputs() +{ + /* multirotor controls */ + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] + * _mc_roll_weight; // roll + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * + _mc_yaw_weight; // yaw + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + + /* fixed wing controls */ + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); //roll + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] + * (1 - _mc_yaw_weight); // yaw + + // set the fixed wing throttle control + if (_vtol_schedule.flight_mode == FW_MODE) { + // take the throttle value commanded by the fw controller + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + + } else { + // otherwise we may be ramping up the throttle during the transition to fw mode + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; + } +} + +/** +* Disable all multirotor motors when in fw mode. +*/ +void +Standard::set_max_mc(unsigned pwm_value) +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = px4_open(dev, 0); + + if (fd < 0) {PX4_WARN("can't open %s", dev);} + + ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count = _params->vtol_motor_count; + } + + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {PX4_WARN("failed setting max values");} + + px4_close(fd); +} diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h new file mode 100644 index 000000000000..ed37ccc3988d --- /dev/null +++ b/src/modules/vtol_att_control/standard.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file standard.h +* VTOL with fixed multirotor motor configurations (such as quad) and a pusher +* (or puller aka tractor) motor for forward flight. +* +* @author Simon Wilks +* @author Roman Bapst +* +*/ + +#ifndef STANDARD_H +#define STANDARD_H +#include "vtol_type.h" +#include +#include + +class Standard : public VtolType +{ + +public: + + Standard(VtolAttitudeControl *_att_controller); + ~Standard(); + + void update_vtol_state(); + void update_mc_state(); + void update_fw_state(); + void update_transition_state(); + void update_external_state(); + +private: + + struct { + float front_trans_dur; + float back_trans_dur; + float pusher_trans; + float airspeed_blend; + float airspeed_trans; + } _params_standard; + + struct { + param_t front_trans_dur; + param_t back_trans_dur; + param_t pusher_trans; + param_t airspeed_blend; + param_t airspeed_trans; + } _params_handles_standard; + + enum vtol_mode { + MC_MODE = 0, + TRANSITION_TO_FW, + TRANSITION_TO_MC, + FW_MODE + }; + + struct { + vtol_mode flight_mode; // indicates in which mode the vehicle is in + hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) + } _vtol_schedule; + + bool _flag_enable_mc_motors; + float _pusher_throttle; + float _airspeed_trans_blend_margin; + + void fill_actuator_outputs(); + void set_max_mc(unsigned pwm_value); + + int parameters_update(); + +}; +#endif diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h b/src/modules/vtol_att_control/standard_params.c old mode 100755 new mode 100644 similarity index 68% rename from src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h rename to src/modules/vtol_att_control/standard_params.c index dfb4cad05a62..3a2185eead13 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h +++ b/src/modules/vtol_att_control/standard_params.c @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Hyon Lim - * Anton Babushkin + * Copyright (c) 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,35 +31,19 @@ * ****************************************************************************/ -/* - * @file attitude_estimator_so3_params.h - * - * Parameters for nonlinear complementary filters on the SO(3). - */ - -#include - -struct attitude_estimator_so3_params { - float Kp; - float Ki; - float roll_off; - float pitch_off; - float yaw_off; -}; - -struct attitude_estimator_so3_param_handles { - param_t Kp, Ki; - param_t roll_off, pitch_off, yaw_off; -}; - /** - * Initialize all parameter handles and values + * @file standard_params.c + * Parameters for the standard VTOL controller. * + * @author Simon Wilks + * @author Roman Bapst */ -int parameters_init(struct attitude_estimator_so3_param_handles *h); /** - * Update all parameters + * Target throttle value for pusher/puller motor during the transition to fw mode * + * @min 0.0 + * @max 1.0 + * @group VTOL Attitude Control */ -int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p); +PARAM_DEFINE_FLOAT(VT_TRANS_THR, 0.6f); diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp new file mode 100644 index 000000000000..736887084736 --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file tailsitter.cpp +* +* @author Roman Bapst +* +*/ + +#include "tailsitter.h" +#include "vtol_att_control_main.h" + +Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) : + VtolType(att_controller), + _airspeed_tot(0), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) +{ + +} + +Tailsitter::~Tailsitter() +{ + +} + +void Tailsitter::update_vtol_state() +{ + // simply switch between the two modes + if (!_attc->is_fixed_wing_requested()) { + _vtol_mode = ROTARY_WING; + + } else { + _vtol_mode = FIXED_WING; + } +} + +void Tailsitter::update_mc_state() +{ + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } +} + +void Tailsitter::update_fw_state() +{ + if (flag_idle_mc) { + set_idle_fw(); + flag_idle_mc = false; + } +} + +void Tailsitter::update_transition_state() +{ + +} + +void Tailsitter::update_external_state() +{ + +} + +void Tailsitter::calc_tot_airspeed() +{ + float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s); // prevent numerical drama + // calculate momentary power of one engine + float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count; + P = math::constrain(P, 1.0f, _params->power_max); + // calculate prop efficiency + float power_factor = 1.0f - P * _params->prop_eff / _params->power_max; + float eta = (1.0f / (1 + expf(-0.4f * power_factor * airspeed)) - 0.5f) * 2.0f; + eta = math::constrain(eta, 0.001f, 1.0f); // live on the safe side + // calculate induced airspeed by propeller + float v_ind = (airspeed / eta - airspeed) * 2.0f; + // calculate total airspeed + float airspeed_raw = airspeed + v_ind; + // apply low-pass filter + _airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; +} + +void +Tailsitter::scale_mc_output() +{ + // scale around tuning airspeed + float airspeed; + calc_tot_airspeed(); // estimate air velocity seen by elevons + + // if airspeed is not updating, we assume the normal average speed + if (bool nonfinite = !PX4_ISFINITE(_airspeed->true_airspeed_m_s) || + hrt_elapsed_time(&_airspeed->timestamp) > 1e6) { + airspeed = _params->mc_airspeed_trim; + + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } + + } else { + airspeed = _airspeed_tot; + airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max); + } + + _vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : + airspeed); + _actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling, + -1.0f, 1.0f); +} + +/** +* Write data to actuator output topic. +*/ +void Tailsitter::fill_actuator_outputs() +{ + switch (_vtol_mode) { + case ROTARY_WING: + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; + + } else { + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + } + + break; + + case FIXED_WING: + // in fixed wing mode we use engines only for providing thrust, no moments are generated + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = + _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + break; + + case TRANSITION: + case EXTERNAL: + // not yet implemented, we are switching brute force at the moment + break; + } +} diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h new file mode 100644 index 000000000000..ec8d007567b8 --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file tiltrotor.h +* +* @author Roman Bapst +* +*/ + +#ifndef TAILSITTER_H +#define TAILSITTER_H + +#include "vtol_type.h" +#include + +class Tailsitter : public VtolType +{ + +public: + Tailsitter(VtolAttitudeControl *_att_controller); + ~Tailsitter(); + + void update_vtol_state(); + void update_mc_state(); + void update_fw_state(); + void update_transition_state(); + void update_external_state(); + +private: + void fill_actuator_outputs(); + void calc_tot_airspeed(); + void scale_mc_output(); + + float _airspeed_tot; + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + +}; +#endif diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp new file mode 100644 index 000000000000..270aa065a927 --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -0,0 +1,453 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tiltrotor.cpp + * + * @author Roman Bapst + * +*/ + +#include "tiltrotor.h" +#include "vtol_att_control_main.h" + +#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition + +Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : + VtolType(attc), + _rear_motors(ENABLED), + _tilt_control(0.0f), + _min_front_trans_dur(0.5f) +{ + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); + _params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS"); + _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); + _params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND"); + _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); + _params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFF"); +} + +Tiltrotor::~Tiltrotor() +{ + +} + +int +Tiltrotor::parameters_update() +{ + float v; + int l; + + /* motors that must be turned off when in fixed wing mode */ + param_get(_params_handles_tiltrotor.fw_motors_off, &l); + _params_tiltrotor.fw_motors_off = get_motor_off_channels(l); + + + /* vtol duration of a front transition */ + param_get(_params_handles_tiltrotor.front_trans_dur, &v); + _params_tiltrotor.front_trans_dur = math::constrain(v, 1.0f, 5.0f); + + /* vtol duration of a back transition */ + param_get(_params_handles_tiltrotor.back_trans_dur, &v); + _params_tiltrotor.back_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* vtol tilt mechanism position in mc mode */ + param_get(_params_handles_tiltrotor.tilt_mc, &v); + _params_tiltrotor.tilt_mc = v; + + /* vtol tilt mechanism position in transition mode */ + param_get(_params_handles_tiltrotor.tilt_transition, &v); + _params_tiltrotor.tilt_transition = v; + + /* vtol tilt mechanism position in fw mode */ + param_get(_params_handles_tiltrotor.tilt_fw, &v); + _params_tiltrotor.tilt_fw = v; + + /* vtol airspeed at which it is ok to switch to fw mode */ + param_get(_params_handles_tiltrotor.airspeed_trans, &v); + _params_tiltrotor.airspeed_trans = v; + + /* vtol airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_tiltrotor.airspeed_blend_start, &v); + _params_tiltrotor.airspeed_blend_start = v; + + /* vtol lock elevons in multicopter */ + param_get(_params_handles_tiltrotor.elevons_mc_lock, &l); + _params_tiltrotor.elevons_mc_lock = l; + + /* vtol front transition phase 2 duration */ + param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); + _params_tiltrotor.front_trans_dur_p2 = v; + + /* avoid parameters which will lead to zero division in the transition code */ + _params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur); + + if (_params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f) { + _params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f; + } + + return OK; +} + +int Tiltrotor::get_motor_off_channels(int channels) +{ + int channel_bitmap = 0; + + int channel; + + for (int i = 0; i < _params->vtol_motor_count; ++i) { + channel = channels % 10; + + if (channel == 0) { + break; + } + + channel_bitmap |= 1 << channel; + channels = channels / 10; + } + + return channel_bitmap; +} + +void Tiltrotor::update_vtol_state() +{ + parameters_update(); + + /* simple logic using a two way switch to perform transitions. + * after flipping the switch the vehicle will start tilting rotors, picking up + * forward speed. After the vehicle has picked up enough speed the rotors are tilted + * forward completely. For the backtransition the motors simply rotate back. + */ + + if (!_attc->is_fixed_wing_requested()) { + + // plane is in multicopter mode + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + break; + + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_FRONT_P2: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_BACK: + if (_tilt_control <= _params_tiltrotor.tilt_mc) { + _vtol_schedule.flight_mode = MC_MODE; + } + + break; + } + + } else { + + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case FW_MODE: + break; + + case TRANSITION_FRONT_P1: + + // check if we have reached airspeed to switch to fw mode + // also allow switch if we are not armed for the sake of bench testing + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans || !_armed->armed) { + _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; + _vtol_schedule.transition_start = hrt_absolute_time(); + } + + break; + + case TRANSITION_FRONT_P2: + + // if the rotors have been tilted completely we switch to fw mode + if (_tilt_control >= _params_tiltrotor.tilt_fw) { + _vtol_schedule.flight_mode = FW_MODE; + _tilt_control = _params_tiltrotor.tilt_fw; + } + + break; + + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + break; + } + } + + // map tiltrotor specific control phases to simple control modes + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + break; + + case FW_MODE: + _vtol_mode = FIXED_WING; + break; + + case TRANSITION_FRONT_P1: + case TRANSITION_FRONT_P2: + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + break; + } +} + +void Tiltrotor::update_mc_state() +{ + // make sure motors are not tilted + _tilt_control = _params_tiltrotor.tilt_mc; + + // enable rear motors + if (_rear_motors != ENABLED) { + set_rear_motor_state(ENABLED); + } + + // set idle speed for rotary wing mode + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; +} + +void Tiltrotor::update_fw_state() +{ + // make sure motors are tilted forward + _tilt_control = _params_tiltrotor.tilt_fw; + + // disable rear motors + if (_rear_motors != DISABLED) { + set_rear_motor_state(DISABLED); + } + + // adjust idle for fixed wing flight + if (flag_idle_mc) { + set_idle_fw(); + flag_idle_mc = false; + } + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; +} + +void Tiltrotor::update_transition_state() +{ + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + // for the first part of the transition the rear rotors are enabled + if (_rear_motors != ENABLED) { + set_rear_motor_state(ENABLED); + } + + // tilt rotors forward up to certain angle + if (_tilt_control <= _params_tiltrotor.tilt_transition) { + _tilt_control = _params_tiltrotor.tilt_mc + + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f); + } + + // do blending of mc and fw controls + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) { + _mc_roll_weight = 0.0f; + + } else { + // at low speeds give full weight to mc + _mc_roll_weight = 1.0f; + } + + // disable mc yaw control once the plane has picked up speed + _mc_yaw_weight = 1.0f; + + if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { + _mc_yaw_weight = 0.0f; + } + + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { + // the plane is ready to go into fixed wing mode, tilt the rotors forward completely + _tilt_control = _params_tiltrotor.tilt_transition + + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + _mc_roll_weight = 0.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + if (_rear_motors != IDLE) { + set_rear_motor_state(IDLE); + } + + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } + + // tilt rotors back + if (_tilt_control > _params_tiltrotor.tilt_mc) { + _tilt_control = _params_tiltrotor.tilt_fw - + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f); + } + + // set zero throttle for backtransition otherwise unwanted moments will be created + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + + _mc_roll_weight = 0.0f; + + } + + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); +} + +void Tiltrotor::update_external_state() +{ + +} + +/** +* Write data to actuator output topic. +*/ +void Tiltrotor::fill_actuator_outputs() +{ + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] + * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * + _mc_yaw_weight; + + if (_vtol_schedule.flight_mode == FW_MODE) { + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + + } else { + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];; + } + + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] + * (1 - _mc_yaw_weight); // yaw + _actuators_out_1->control[4] = _tilt_control; +} + + +/** +* Set state of rear motors. +*/ + +void Tiltrotor::set_rear_motor_state(rear_motor_state state) +{ + int pwm_value = PWM_DEFAULT_MAX; + + // map desired rear rotor state to max allowed pwm signal + switch (state) { + case ENABLED: + pwm_value = PWM_DEFAULT_MAX; + _rear_motors = ENABLED; + break; + + case DISABLED: + pwm_value = PWM_LOWEST_MAX; + _rear_motors = DISABLED; + break; + + case IDLE: + pwm_value = _params->idle_pwm_mc; + _rear_motors = IDLE; + break; + } + + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = px4_open(dev, 0); + + if (fd < 0) {PX4_WARN("can't open %s", dev);} + + ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + if (is_motor_off_channel(i)) { + pwm_values.values[i] = pwm_value; + + } else { + pwm_values.values[i] = PWM_DEFAULT_MAX; + } + + pwm_values.channel_count = _params->vtol_motor_count; + } + + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {PX4_WARN("failed setting max values");} + + px4_close(fd); +} + +bool Tiltrotor::is_motor_off_channel(const int channel) +{ + return (_params_tiltrotor.fw_motors_off >> channel) & 1; +} diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h new file mode 100644 index 000000000000..507920cb42da --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file tiltrotor.h +* +* @author Roman Bapst +* +*/ + +#ifndef TILTROTOR_H +#define TILTROTOR_H +#include "vtol_type.h" +#include +#include + +class Tiltrotor : public VtolType +{ + +public: + + Tiltrotor(VtolAttitudeControl *_att_controller); + ~Tiltrotor(); + + /** + * Update vtol state. + */ + void update_vtol_state(); + + /** + * Update multicopter state. + */ + void update_mc_state(); + + /** + * Update fixed wing state. + */ + void update_fw_state(); + + /** + * Update transition state. + */ + void update_transition_state(); + + /** + * Update external state. + */ + void update_external_state(); + +private: + + struct { + float front_trans_dur; /**< duration of first part of front transition */ + float back_trans_dur; /**< duration of back transition */ + float tilt_mc; /**< actuator value corresponding to mc tilt */ + float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */ + float tilt_fw; /**< actuator value corresponding to fw tilt */ + float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ + float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ + int elevons_mc_lock; /**< lock elevons in multicopter mode */ + float front_trans_dur_p2; + int fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ + } _params_tiltrotor; + + struct { + param_t front_trans_dur; + param_t back_trans_dur; + param_t tilt_mc; + param_t tilt_transition; + param_t tilt_fw; + param_t airspeed_trans; + param_t airspeed_blend_start; + param_t elevons_mc_lock; + param_t front_trans_dur_p2; + param_t fw_motors_off; + } _params_handles_tiltrotor; + + enum vtol_mode { + MC_MODE = 0, /**< vtol is in multicopter mode */ + TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ + TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */ + TRANSITION_BACK, /**< vtol is in back transition mode */ + FW_MODE /**< vtol is in fixed wing mode */ + }; + + /** + * Specific to tiltrotor with vertical aligned rear engine/s. + * These engines need to be shut down in fw mode. During the back-transition + * they need to idle otherwise they need too much time to spin up for mc mode. + */ + enum rear_motor_state { + ENABLED = 0, + DISABLED, + IDLE + } _rear_motors; + + struct { + vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ + hrt_abstime transition_start; /**< absoulte time at which front transition started */ + } _vtol_schedule; + + float _tilt_control; /**< actuator value for the tilt servo */ + + const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + + /** + * Return a bitmap of channels that should be turned off in fixed wing mode. + */ + int get_motor_off_channels(const int channels); + + /** + * Return true if the motor channel is off in fixed wing mode. + */ + bool is_motor_off_channel(const int channel); + + /** + * Write control values to actuator output topics. + */ + void fill_actuator_outputs(); + + /** + * Adjust the state of the rear motors. In fw mode they shouldn't spin. + */ + void set_rear_motor_state(rear_motor_state state); + + /** + * Update parameters. + */ + int parameters_update(); + +}; +#endif diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c new file mode 100644 index 000000000000..58062c40b014 --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tiltrotor_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + */ + +#include + +/** + * Position of tilt servo in mc mode + * + * Position of tilt servo in mc mode + * + * @min 0.0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f); + +/** + * Position of tilt servo in transition mode + * + * Position of tilt servo in transition mode + * + * @min 0.0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f); + +/** + * Position of tilt servo in fw mode + * + * Position of tilt servo in fw mode + * + * @min 0.0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); + +/** + * Duration of front transition phase 2 + * + * Time in seconds it should take for the rotors to rotate forward completely from the point + * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + * + * @min 0.1 + * @max 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); + +/** + * The channel number of motors that must be turned off in fixed wing mode. + * + * + * @min 0 + * @max 123456 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_MOT_OFF, 0); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e3625bb1be3c..0248a2072736 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -43,166 +43,7 @@ * @author Thomas Gubler * */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "drivers/drv_pwm_output.h" -#include - -#include - - -extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); - -class VtolAttitudeControl -{ -public: - - VtolAttitudeControl(); - ~VtolAttitudeControl(); - - int start(); /* start the task and return OK on success */ - - -private: -//******************flags & handlers****************************************************** - bool _task_should_exit; - int _control_task; //task handle for VTOL attitude controller - - /* handlers for subscriptions */ - int _v_att_sub; //vehicle attitude subscription - int _v_att_sp_sub; //vehicle attitude setpoint subscription - int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription - int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription - int _v_control_mode_sub; //vehicle control mode subscription - int _params_sub; //parameter updates subscription - int _manual_control_sp_sub; //manual control setpoint subscription - int _armed_sub; //arming status subscription - int _local_pos_sub; // sensor subscription - int _airspeed_sub; // airspeed subscription - int _battery_status_sub; // battery status subscription - - int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs - int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs - - //handlers for publishers - orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) - orb_advert_t _actuators_1_pub; - orb_advert_t _vtol_vehicle_status_pub; - orb_advert_t _v_rates_sp_pub; -//*******************data containers*********************************************************** - struct vehicle_attitude_s _v_att; //vehicle attitude - struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint - struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint - struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint - struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint - struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint - struct vehicle_control_mode_s _v_control_mode; //vehicle control mode - struct vtol_vehicle_status_s _vtol_vehicle_status; - struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer - struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control - struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control - struct actuator_armed_s _armed; //actuator arming status - struct vehicle_local_position_s _local_pos; - struct airspeed_s _airspeed; // airspeed - struct battery_status_s _batt_status; // battery status - - struct { - param_t idle_pwm_mc; //pwm value for idle in mc mode - param_t vtol_motor_count; - param_t vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode - float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) - float mc_airspeed_trim; // trim airspeed in multicopter mode - float mc_airspeed_max; // max airpseed in multicopter mode - float fw_pitch_trim; // trim for neutral elevon position in fw mode - float power_max; // maximum power of one engine - float prop_eff; // factor to calculate prop efficiency - float arsp_lp_gain; // total airspeed estimate low pass gain - } _params; - - struct { - param_t idle_pwm_mc; - param_t vtol_motor_count; - param_t vtol_fw_permanent_stab; - param_t mc_airspeed_min; - param_t mc_airspeed_trim; - param_t mc_airspeed_max; - param_t fw_pitch_trim; - param_t power_max; - param_t prop_eff; - param_t arsp_lp_gain; - } _params_handles; - - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ - - /* for multicopters it is usual to have a non-zero idle speed of the engines - * for fixed wings we want to have an idle speed of zero since we do not want - * to waste energy when gliding. */ - bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" - unsigned _motor_count; // number of motors - float _airspeed_tot; - float _tilt_control; -//*****************Member functions*********************************************************************** - - void task_main(); //main task - static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. - - void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - void vehicle_manual_poll(); //Check for changes in manual inputs. - void arming_status_poll(); //Check for arming status updates. - void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output - void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - void vehicle_rates_sp_mc_poll(); - void vehicle_rates_sp_fw_poll(); - void vehicle_local_pos_poll(); // Check for changes in sensor values - void vehicle_airspeed_poll(); // Check for changes in airspeed - void vehicle_battery_poll(); // Check for battery updates - void parameters_update_poll(); //Check if parameters have changed - int parameters_update(); //Update local paraemter cache - void fill_mc_att_control_output(); //write mc_att_control results to actuator message - void fill_fw_att_control_output(); //write fw_att_control results to actuator message - void fill_mc_att_rates_sp(); - void fill_fw_att_rates_sp(); - void set_idle_fw(); - void set_idle_mc(); - void scale_mc_output(); - void calc_tot_airspeed(); // estimated airspeed seen by elevons -}; +#include "vtol_att_control_main.h" namespace VTOL_att_control { @@ -228,21 +69,15 @@ VtolAttitudeControl::VtolAttitudeControl() : _local_pos_sub(-1), _airspeed_sub(-1), _battery_status_sub(-1), + _vehicle_cmd_sub(-1), //init publication handlers _actuators_0_pub(nullptr), _actuators_1_pub(nullptr), _vtol_vehicle_status_pub(nullptr), - _v_rates_sp_pub(nullptr), + _v_rates_sp_pub(nullptr) - _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input")) { - - flag_idle_mc = true; - _airspeed_tot = 0.0f; - _tilt_control = 0.0f; - memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); @@ -258,9 +93,10 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); memset(&_armed, 0, sizeof(_armed)); - memset(&_local_pos,0,sizeof(_local_pos)); - memset(&_airspeed,0,sizeof(_airspeed)); - memset(&_batt_status,0,sizeof(_batt_status)); + memset(&_local_pos, 0, sizeof(_local_pos)); + memset(&_airspeed, 0, sizeof(_airspeed)); + memset(&_batt_status, 0, sizeof(_batt_status)); + memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; @@ -276,9 +112,27 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.power_max = param_find("VT_POWER_MAX"); _params_handles.prop_eff = param_find("VT_PROP_EFF"); _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); + _params_handles.vtol_type = param_find("VT_TYPE"); + _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); /* fetch initial parameter values */ parameters_update(); + + if (_params.vtol_type == 0) { + _tailsitter = new Tailsitter(this); + _vtol_type = _tailsitter; + + } else if (_params.vtol_type == 1) { + _tiltrotor = new Tiltrotor(this); + _vtol_type = _tiltrotor; + + } else if (_params.vtol_type == 2) { + _standard = new Standard(this); + _vtol_type = _standard; + + } else { + _task_should_exit = true; + } } /** @@ -299,7 +153,7 @@ VtolAttitudeControl::~VtolAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -407,7 +261,8 @@ void VtolAttitudeControl::vehicle_rates_sp_fw_poll() * Check for airspeed updates. */ void -VtolAttitudeControl::vehicle_airspeed_poll() { +VtolAttitudeControl::vehicle_airspeed_poll() +{ bool updated; orb_check(_airspeed_sub, &updated); @@ -420,7 +275,8 @@ VtolAttitudeControl::vehicle_airspeed_poll() { * Check for battery updates. */ void -VtolAttitudeControl::vehicle_battery_poll() { +VtolAttitudeControl::vehicle_battery_poll() +{ bool updated; orb_check(_battery_status_sub, &updated); @@ -463,6 +319,49 @@ VtolAttitudeControl::vehicle_local_pos_poll() } +/** +* Check for command updates. +*/ +void +VtolAttitudeControl::vehicle_cmd_poll() +{ + bool updated; + orb_check(_vehicle_cmd_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub , &_vehicle_cmd); + handle_command(); + } +} + +/** +* Check received command +*/ +void +VtolAttitudeControl::handle_command() +{ + // update transition command if necessary + if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { + _transition_command = int(_vehicle_cmd.param1 + 0.5f); + } +} + +/* + * Returns true if fixed-wing mode is requested. + * Changed either via switch or via command. + */ +bool +VtolAttitudeControl::is_fixed_wing_requested() +{ + bool to_fw = _manual_control_sp.aux1 > 0.0f; + + if (_v_control_mode.flag_control_offboard_enabled) { + to_fw = _transition_command == vehicle_status_s::VEHICLE_VTOL_STATE_FW; + } + + return to_fw; +} + /** * Update parameters. */ @@ -470,6 +369,7 @@ int VtolAttitudeControl::parameters_update() { float v; + int l; /* idle pwm for mc mode */ param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc); @@ -507,40 +407,14 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.arsp_lp_gain, &v); _params.arsp_lp_gain = v; - return OK; -} + param_get(_params_handles.vtol_type, &l); + _params.vtol_type = l; -/** -* Prepare message to acutators with data from mc attitude controller. -*/ -void VtolAttitudeControl::fill_mc_att_control_output() -{ - _actuators_out_0.control[0] = _actuators_mc_in.control[0]; - _actuators_out_0.control[1] = _actuators_mc_in.control[1]; - _actuators_out_0.control[2] = _actuators_mc_in.control[2]; - _actuators_out_0.control[3] = _actuators_mc_in.control[3]; - //set neutral position for elevons - _actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon - _actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon - _actuators_out_1.control[4] = _tilt_control; // for tilt-rotor control -} + /* vtol lock elevons in multicopter */ + param_get(_params_handles.elevons_mc_lock, &l); + _params.elevons_mc_lock = l; -/** -* Prepare message to acutators with data from fw attitude controller. -*/ -void VtolAttitudeControl::fill_fw_att_control_output() -{ - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0.control[0] = 0; - _actuators_out_0.control[1] = 0; - _actuators_out_0.control[2] = 0; - _actuators_out_0.control[3] = _actuators_fw_in.control[3]; - /*controls for the elevons */ - _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; // roll elevon - _actuators_out_1.control[1] = _actuators_fw_in.control[1] + _params.fw_pitch_trim; // pitch elevon - // unused now but still logged - _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw - _actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle + return OK; } /** @@ -565,109 +439,6 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; } -/** -* Adjust idle speed for fw mode. -*/ -void VtolAttitudeControl::set_idle_fw() -{ - int ret; - char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - unsigned pwm_value = PWM_LOWEST_MIN; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < _params.vtol_motor_count; i++) { - - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -/** -* Adjust idle speed for mc mode. -*/ -void VtolAttitudeControl::set_idle_mc() -{ - int ret; - unsigned servo_count; - char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - unsigned pwm_value = _params.idle_pwm_mc; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < _params.vtol_motor_count; i++) { - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -void -VtolAttitudeControl::scale_mc_output() { - // scale around tuning airspeed - float airspeed; - calc_tot_airspeed(); // estimate air velocity seen by elevons - // if airspeed is not updating, we assume the normal average speed - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { - airspeed = _params.mc_airspeed_trim; - if (nonfinite) { - perf_count(_nonfinite_input_perf); - } - } else { - airspeed = _airspeed_tot; - airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max); - } - - _vtol_vehicle_status.airspeed_tot = airspeed; // save value for logging - /* - * For scaling our actuators using anything less than the min (close to stall) - * speed doesn't make any sense - its the strongest reasonable deflection we - * want to do in flight and its the baseline a human pilot would choose. - * - * Forcing the scaling to this value allows reasonable handheld tests. - */ - float airspeed_scaling = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed); - _actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); -} - -void VtolAttitudeControl::calc_tot_airspeed() { - float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s); // prevent numerical drama - // calculate momentary power of one engine - float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count; - P = math::constrain(P,1.0f,_params.power_max); - // calculate prop efficiency - float power_factor = 1.0f - P*_params.prop_eff/_params.power_max; - float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f; - eta = math::constrain(eta,0.001f,1.0f); // live on the safe side - // calculate induced airspeed by propeller - float v_ind = (airspeed/eta - airspeed)*2.0f; - // calculate total airspeed - float airspeed_raw = airspeed + v_ind; - // apply low-pass filter - _airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; -} - void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -676,7 +447,7 @@ VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) void VtolAttitudeControl::task_main() { - warnx("started"); + PX4_WARN("started"); fflush(stdout); /* do subscriptions */ @@ -691,6 +462,7 @@ void VtolAttitudeControl::task_main() _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); + _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -701,11 +473,10 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; // make sure we start with idle in mc mode - set_idle_mc(); - flag_idle_mc = true; + _vtol_type->set_idle_mc(); /* wakeup source*/ - struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ + px4_pollfd_struct_t fds[3]; /*input_mc, input_fw, parameters*/ fds[0].fd = _actuator_inputs_mc; fds[0].events = POLLIN; @@ -725,7 +496,7 @@ void VtolAttitudeControl::task_main() } /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ @@ -763,97 +534,111 @@ void VtolAttitudeControl::task_main() vehicle_local_pos_poll(); // Check for new sensor values vehicle_airspeed_poll(); vehicle_battery_poll(); + vehicle_cmd_poll(); + // update the vtol state machine which decides which mode we are in + _vtol_type->update_vtol_state(); - if (_manual_control_sp.aux1 < 0.0f) { /* vehicle is in mc mode */ - _vtol_vehicle_status.vtol_in_rw_mode = true; + // reset transition command if not in offboard control + if (!_v_control_mode.flag_control_offboard_enabled) { + if (_vtol_type->get_mode() == ROTARY_WING) { + _transition_command = vehicle_status_s::VEHICLE_VTOL_STATE_MC; - if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */ - set_idle_mc(); - flag_idle_mc = true; + } else if (_vtol_type->get_mode() == FIXED_WING) { + _transition_command = vehicle_status_s::VEHICLE_VTOL_STATE_FW; } + } + + // check in which mode we are in and call mode specific functions + if (_vtol_type->get_mode() == ROTARY_WING) { + // vehicle is in rotary wing mode + _vtol_vehicle_status.vtol_in_rw_mode = true; + _vtol_vehicle_status.vtol_in_trans_mode = false; - /* got data from mc_att_controller */ + // got data from mc attitude controller if (fds[0].revents & POLLIN) { - vehicle_manual_poll(); /* update remote input */ orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - // scale pitch control with total airspeed - scale_mc_output(); + _vtol_type->update_mc_state(); - fill_mc_att_control_output(); fill_mc_att_rates_sp(); + } - /* Only publish if the proper mode(s) are enabled */ - if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled) - { - if (_actuators_0_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + } else if (_vtol_type->get_mode() == FIXED_WING) { + // vehicle is in fw mode + _vtol_vehicle_status.vtol_in_rw_mode = false; + _vtol_vehicle_status.vtol_in_trans_mode = false; - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } + // got data from fw attitude controller + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); + vehicle_manual_poll(); - if (_actuators_1_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + _vtol_type->update_fw_state(); - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } - } + fill_fw_att_rates_sp(); } - } - if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */ - _vtol_vehicle_status.vtol_in_rw_mode = false; + } else if (_vtol_type->get_mode() == TRANSITION) { + // vehicle is doing a transition + _vtol_vehicle_status.vtol_in_trans_mode = true; - if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */ - set_idle_fw(); - flag_idle_mc = false; + bool got_new_data = false; + + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); + got_new_data = true; } - if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */ + if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); - vehicle_manual_poll(); //update remote input + got_new_data = true; + } - fill_fw_att_control_output(); - fill_fw_att_rates_sp(); + // update transition state if got any new data + if (got_new_data) { + _vtol_type->update_transition_state(); + fill_mc_att_rates_sp(); + } + + } else if (_vtol_type->get_mode() == EXTERNAL) { + // we are using external module to generate attitude/thrust setpoint + _vtol_type->update_external_state(); + } - /* Only publish if the proper mode(s) are enabled */ - if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled || - _v_control_mode.flag_control_manual_enabled) - { - if (_actuators_0_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } - } + _vtol_type->fill_actuator_outputs(); + + /* Only publish if the proper mode(s) are enabled */ + if (_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled || + _v_control_mode.flag_control_manual_enabled) { + if (_actuators_0_pub != nullptr) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } + + if (_actuators_1_pub != nullptr) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); } } // publish the attitude rates setpoint - if(_v_rates_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_rates_setpoint),_v_rates_sp_pub,&_v_rates_sp); - } - else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),&_v_rates_sp); + if (_v_rates_sp_pub != nullptr) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); } } warnx("exit"); _control_task = -1; - _exit(0); + return; } int @@ -863,14 +648,14 @@ VtolAttitudeControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("vtol_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 2048, - (main_t)&VtolAttitudeControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + (px4_main_t)&VtolAttitudeControl::task_main_trampoline, + nullptr); if (_control_task < 0) { - warn("task start failed"); + PX4_WARN("task start failed"); return -errno; } @@ -881,49 +666,55 @@ VtolAttitudeControl::start() int vtol_att_control_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: vtol_att_control {start|stop|status}"); + PX4_WARN("usage: vtol_att_control {start|stop|status}"); } if (!strcmp(argv[1], "start")) { if (VTOL_att_control::g_control != nullptr) { - errx(1, "already running"); + PX4_WARN("already running"); + return 0; } VTOL_att_control::g_control = new VtolAttitudeControl; if (VTOL_att_control::g_control == nullptr) { - errx(1, "alloc failed"); + PX4_WARN("alloc failed"); + return 1; } if (OK != VTOL_att_control::g_control->start()) { delete VTOL_att_control::g_control; VTOL_att_control::g_control = nullptr; - err(1, "start failed"); + PX4_WARN("start failed"); + return 1; } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { if (VTOL_att_control::g_control == nullptr) { - errx(1, "not running"); + PX4_WARN("not running"); + return 0; } delete VTOL_att_control::g_control; VTOL_att_control::g_control = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (VTOL_att_control::g_control) { - errx(0, "running"); + PX4_WARN("running"); } else { - errx(1, "not running"); + PX4_WARN("not running"); } + + return 0; } - warnx("unrecognized command"); + PX4_WARN("unrecognized command"); return 1; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h new file mode 100644 index 000000000000..c10f7b0dd750 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VTOL_att_control_main.cpp + * Implementation of an attitude controller for VTOL airframes. This module receives data + * from both the fixed wing- and the multicopter attitude controllers and processes it. + * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward- + * flight or transition). It also publishes the resulting controls on the actuator controls topics. + * + * @author Roman Bapst + * @author Lorenz Meier + * @author Thomas Gubler + * + */ +#ifndef VTOL_ATT_CONTROL_MAIN_H +#define VTOL_ATT_CONTROL_MAIN_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tiltrotor.h" +#include "tailsitter.h" +#include "standard.h" + + +extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); + + +class VtolAttitudeControl +{ +public: + + VtolAttitudeControl(); + ~VtolAttitudeControl(); + + int start(); /* start the task and return OK on success */ + bool is_fixed_wing_requested(); + + struct vehicle_attitude_s *get_att() {return &_v_att;} + struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} + struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} + struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} + struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} + struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} + struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} + struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} + struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} + struct actuator_armed_s *get_armed() {return &_armed;} + struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} + struct airspeed_s *get_airspeed() {return &_airspeed;} + struct battery_status_s *get_batt_status() {return &_batt_status;} + + struct Params *get_params() {return &_params;} + + +private: +//******************flags & handlers****************************************************** + bool _task_should_exit; + int _control_task; //task handle for VTOL attitude controller + + /* handlers for subscriptions */ + int _v_att_sub; //vehicle attitude subscription + int _v_att_sp_sub; //vehicle attitude setpoint subscription + int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription + int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription + int _v_control_mode_sub; //vehicle control mode subscription + int _params_sub; //parameter updates subscription + int _manual_control_sp_sub; //manual control setpoint subscription + int _armed_sub; //arming status subscription + int _local_pos_sub; // sensor subscription + int _airspeed_sub; // airspeed subscription + int _battery_status_sub; // battery status subscription + int _vehicle_cmd_sub; + + int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs + int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs + + //handlers for publishers + orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) + orb_advert_t _actuators_1_pub; + orb_advert_t _vtol_vehicle_status_pub; + orb_advert_t _v_rates_sp_pub; +//*******************data containers*********************************************************** + struct vehicle_attitude_s _v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint + struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint + struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint + struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s _v_control_mode; //vehicle control mode + struct vtol_vehicle_status_s _vtol_vehicle_status; + struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s _armed; //actuator arming status + struct vehicle_local_position_s _local_pos; + struct airspeed_s _airspeed; // airspeed + struct battery_status_s _batt_status; // battery status + struct vehicle_command_s _vehicle_cmd; + + Params _params; // struct holding the parameters + + struct { + param_t idle_pwm_mc; + param_t vtol_motor_count; + param_t vtol_fw_permanent_stab; + param_t mc_airspeed_min; + param_t mc_airspeed_trim; + param_t mc_airspeed_max; + param_t fw_pitch_trim; + param_t power_max; + param_t prop_eff; + param_t arsp_lp_gain; + param_t vtol_type; + param_t elevons_mc_lock; + } _params_handles; + + int _transition_command; + + VtolType *_vtol_type; // base class for different vtol types + Tiltrotor *_tiltrotor; // tailsitter vtol type + Tailsitter *_tailsitter; // tiltrotor vtol type + Standard *_standard; // standard vtol type + +//*****************Member functions*********************************************************************** + + void task_main(); //main task + static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. + + void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. + void vehicle_manual_poll(); //Check for changes in manual inputs. + void arming_status_poll(); //Check for arming status updates. + void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output + void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output + void vehicle_rates_sp_mc_poll(); + void vehicle_rates_sp_fw_poll(); + void vehicle_local_pos_poll(); // Check for changes in sensor values + void vehicle_airspeed_poll(); // Check for changes in airspeed + void vehicle_battery_poll(); // Check for battery updates + void vehicle_cmd_poll(); + void parameters_update_poll(); //Check if parameters have changed + int parameters_update(); //Update local paraemter cache + void fill_mc_att_rates_sp(); + void fill_fw_att_rates_sp(); + void handle_command(); +}; + +#endif diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 6da28b13045c..fe7d8ebd82cf 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -35,18 +35,16 @@ * @file vtol_att_control_params.c * Parameters for vtol attitude controller. * - * @author Roman Bapst + * @author Roman Bapst */ -#include - /** * VTOL number of engines * - * @min 1 + * @min 0 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_MOT_COUNT,0); +PARAM_DEFINE_INT32(VT_MOT_COUNT, 0); /** * Idle speed of VTOL when in multicopter mode @@ -54,7 +52,7 @@ PARAM_DEFINE_INT32(VT_MOT_COUNT,0); * @min 900 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); +PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900); /** * Minimum airspeed in multicopter mode @@ -64,7 +62,7 @@ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN, 10.0f); /** * Maximum airspeed in multicopter mode @@ -74,7 +72,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX, 30.0f); /** * Trim airspeed when in multicopter mode @@ -84,7 +82,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM, 10.0f); /** * Permanent stabilization in fw mode @@ -96,7 +94,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); +PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0); /** * Fixed wing pitch trim @@ -107,7 +105,7 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); +PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM, 0.0f); /** * Motor max power @@ -118,18 +116,18 @@ PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); * @min 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_POWER_MAX,120.0f); +PARAM_DEFINE_FLOAT(VT_POWER_MAX, 120.0f); /** * Propeller efficiency parameter * * Influences propeller efficiency at different power settings. Should be tuned beforehand. * - * @min 0.5 + * @min 0.0 * @max 0.9 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); +PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); /** * Total airspeed estimate low-pass filter gain @@ -140,5 +138,68 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); * @max 0.99 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); +PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f); + +/** + * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) + * + * @min 0 + * @max 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_TYPE, 0); + +/** + * Lock elevons in multicopter mode + * + * If set to 1 the elevons are locked in multicopter mode + * + * @min 0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0); + +/** + * Duration of a front transition + * + * Time in seconds used for a transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 3.0f); + +/** + * Duration of a back transition + * + * Time in seconds used for a back transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 2.0f); + +/** + * Transition blending airspeed + * + * Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable. + * + * @min 0.0 + * @max 20.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f); +/** + * Transition airspeed + * + * Airspeed at which we can switch to fw mode + * + * @min 1.0 + * @max 20 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp new file mode 100644 index 000000000000..f57080a2a7bc --- /dev/null +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file airframe.cpp +* +* @author Roman Bapst +* +*/ + +#include "vtol_type.h" +#include "drivers/drv_pwm_output.h" +#include +#include "vtol_att_control_main.h" + +VtolType::VtolType(VtolAttitudeControl *att_controller) : + _attc(att_controller), + _vtol_mode(ROTARY_WING) +{ + _v_att = _attc->get_att(); + _v_att_sp = _attc->get_att_sp(); + _v_rates_sp = _attc->get_rates_sp(); + _mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp(); + _fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp(); + _manual_control_sp = _attc->get_manual_control_sp(); + _v_control_mode = _attc->get_control_mode(); + _vtol_vehicle_status = _attc->get_vehicle_status(); + _actuators_out_0 = _attc->get_actuators_out0(); + _actuators_out_1 = _attc->get_actuators_out1(); + _actuators_mc_in = _attc->get_actuators_mc_in(); + _actuators_fw_in = _attc->get_actuators_fw_in(); + _armed = _attc->get_armed(); + _local_pos = _attc->get_local_pos(); + _airspeed = _attc->get_airspeed(); + _batt_status = _attc->get_batt_status(); + _params = _attc->get_params(); + + flag_idle_mc = true; +} + +VtolType::~VtolType() +{ + +} + +/** +* Adjust idle speed for mc mode. +*/ +void VtolType::set_idle_mc() +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = px4_open(dev, 0); + + if (fd < 0) {PX4_WARN("can't open %s", dev);} + + ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + unsigned pwm_value = _params->idle_pwm_mc; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {PX4_WARN("failed setting min values");} + + px4_close(fd); + + flag_idle_mc = true; +} + +/** +* Adjust idle speed for fw mode. +*/ +void VtolType::set_idle_fw() +{ + int ret; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = px4_open(dev, 0); + + if (fd < 0) {PX4_WARN("can't open %s", dev);} + + unsigned pwm_value = PWM_LOWEST_MIN; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {PX4_WARN("failed setting min values");} + + px4_close(fd); +} diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h new file mode 100644 index 000000000000..d8557110c0ac --- /dev/null +++ b/src/modules/vtol_att_control/vtol_type.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file airframe.h +* +* @author Roman Bapst +* +*/ + +#ifndef VTOL_TYPE_H +#define VTOL_TYPE_H + +#include + +struct Params { + int idle_pwm_mc; // pwm value for idle in mc mode + int vtol_motor_count; // number of motors + int vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode + float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) + float mc_airspeed_trim; // trim airspeed in multicopter mode + float mc_airspeed_max; // max airpseed in multicopter mode + float fw_pitch_trim; // trim for neutral elevon position in fw mode + float power_max; // maximum power of one engine + float prop_eff; // factor to calculate prop efficiency + float arsp_lp_gain; // total airspeed estimate low pass gain + int vtol_type; + int elevons_mc_lock; // lock elevons in multicopter mode +}; + +enum mode { + ROTARY_WING = 0, + FIXED_WING, + TRANSITION, + EXTERNAL +}; + +class VtolAttitudeControl; + +class VtolType +{ +public: + + VtolType(VtolAttitudeControl *att_controller); + + virtual ~VtolType(); + + virtual void update_vtol_state() = 0; + virtual void update_mc_state() = 0; + virtual void update_fw_state() = 0; + virtual void update_transition_state() = 0; + virtual void update_external_state() = 0; + virtual void fill_actuator_outputs() = 0; + + void set_idle_mc(); + void set_idle_fw(); + + mode get_mode() {return _vtol_mode;}; + +protected: + VtolAttitudeControl *_attc; + mode _vtol_mode; + + struct vehicle_attitude_s *_v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint + struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint + struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint + struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode + struct vtol_vehicle_status_s *_vtol_vehicle_status; + struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s *_armed; //actuator arming status + struct vehicle_local_position_s *_local_pos; + struct airspeed_s *_airspeed; // airspeed + struct battery_status_s *_batt_status; // battery status + + struct Params *_params; + + bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" + + float _mc_roll_weight; // weight for multicopter attitude controller roll output + float _mc_pitch_weight; // weight for multicopter attitude controller pitch output + float _mc_yaw_weight; // weight for multicopter attitude controller yaw output + +}; + +#endif From 8ba0e49d0c1d433077630eb4c0c8c08fdb84dc74 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 11:34:36 +1100 Subject: [PATCH 241/293] px4iofirmware: fixed build with new layout --- src/modules/px4iofirmware/CMakeLists.txt | 141 +++++++++++++++++++++++ src/modules/px4iofirmware/controls.c | 2 +- src/modules/px4iofirmware/module.mk | 4 +- 3 files changed, 144 insertions(+), 3 deletions(-) create mode 100644 src/modules/px4iofirmware/CMakeLists.txt diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt new file mode 100644 index 000000000000..854bbdcc80bf --- /dev/null +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -0,0 +1,141 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# kill all flags above us, this is a different board (io) +set_directory_properties(PROPERTIES + INCLUDE_DIRECTORIES "" + LINK_DIRECTORIES "" + COMPILE_DEFINITIONS "" + ) +set(c_flags) +set(exe_linker_flags) +set(cxx_flags) +set(include_dirs) +set(link_dirs) +set(definitions) + +px4_os_prebuild_targets(OUT io_prebuild_targets + BOARD ${config_io_board} + THREADS ${THREADS}) + +px4_os_add_flags( + BOARD ${config_io_board} + C_FLAGS c_flags + CXX_FLAGS cxx_flags + EXE_LINKER_FLAGS exe_linker_flags + INCLUDE_DIRS include_dirs + LINK_DIRS link_dirs + DEFINITIONS definitions) + +px4_join(OUT CMAKE_EXE_LINKER_FLAGS LIST "${exe_linker_flags}" GLUE " ") +px4_join(OUT CMAKE_C_FLAGS LIST "${c_flags}" GLUE " ") +px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ") + +include_directories( + ${include_dirs} + ${CMAKE_BINARY_DIR}/src/modules/systemlib/mixer + . + ) +link_directories(${link_dirs}) +add_definitions(${definitions}) + +set(srcs + adc.c + controls.c + px4io.c + registers.c + safety.c + ../systemlib/up_cxxinitialize.c + ../systemlib/perf_counter.c + mixer.cpp + ../systemlib/mixer/mixer.cpp + ../systemlib/mixer/mixer_group.cpp + ../systemlib/mixer/mixer_multirotor.cpp + ../systemlib/mixer/mixer_simple.cpp + ../systemlib/pwm_limit/pwm_limit.c + ../../lib/rc/st24.c + ../../lib/rc/sumd.c + ../../lib/rc/sbus.c + ../../lib/rc/dsm.c + ../../drivers/stm32/drv_hrt.c + ../../drivers/stm32/drv_pwm_servo.c + ../../drivers/boards/${config_io_board}/px4io_init.c + ../../drivers/boards/${config_io_board}/px4io_pwm_servo.c + ) + +if(${config_io_board} STREQUAL "px4io-v1") + list(APPEND srcs + i2c.c + ) +elseif(${config_io_board} STREQUAL "px4io-v2") + list(APPEND srcs + serial.c + ../systemlib/hx_stream.c + ) +endif() + +set(fw_io_name ${config_io_board}) + +add_executable(${fw_io_name} + ${srcs}) + +add_dependencies(${fw_io_name} + nuttx_export_${config_io_board} + msg_gen + io_prebuild_targets + mixer_gen + ) + +set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${config_io_board}/NuttX/nuttx-export) +set(main_link_flags + "-T${nuttx_export_dir}/build/ld.script" + "-Wl,-Map=${CMAKE_BINARY_DIR}/${config_io_board}/main.map" + ) +px4_join(OUT main_link_flags LIST ${main_link_flags} GLUE " ") +set_target_properties(${fw_io_name} PROPERTIES LINK_FLAGS ${main_link_flags}) + +target_link_libraries(${fw_io_name} + -Wl,--start-group + apps nuttx nosys m gcc + ${config_io_extra_libs} + -Wl,--end-group) + +px4_nuttx_create_bin(OUT ${CMAKE_CURRENT_BINARY_DIR}/${fw_io_name}.bin + EXE ${fw_io_name} + ) + +add_custom_target(fw_io + DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/${fw_io_name}.bin) + + +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 25874868b1d3..2d8e43c8ea45 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -63,7 +63,7 @@ static perf_counter_t c_gather_sbus; static perf_counter_t c_gather_ppm; static int _dsm_fd = -1; -int _sbus_fd = -1; +__EXPORT int _sbus_fd = -1; static uint16_t rc_value_override = 0; diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 6794b20573e8..117c5ff0a792 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -25,5 +25,5 @@ SRCS += serial.c \ ../systemlib/hx_stream.c endif -SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) -include $(SELF_DIR)../systemlib/mixer/multi_tables.mk + + From 144e7978b28583c6e5e68e1e1c7240f69b104abe Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 11:42:05 +1100 Subject: [PATCH 242/293] makefiles: fixed upload to FMUv4 --- makefiles/upload.mk | 3 +++ 1 file changed, 3 insertions(+) diff --git a/makefiles/upload.mk b/makefiles/upload.mk index e73c31dc313c..cc23828b57ce 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -30,6 +30,9 @@ upload-serial-px4fmu-v1: $(BUNDLE) $(UPLOADER) upload-serial-px4fmu-v2: $(BUNDLE) $(UPLOADER) $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) +upload-serial-px4fmu-v4: $(BUNDLE) $(UPLOADER) + $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) + upload-serial-aerocore: openocd -f $(PX4_BASE)/makefiles/nuttx/gumstix-aerocore.cfg -c 'init; reset halt; flash write_image erase $(PX4_BASE)/../Bootloader/px4aerocore_bl.bin 0x08000000; flash write_image erase $(PX4_BASE)/Build/aerocore_default.build/firmware.bin 0x08004000; reset run; exit' From fd8467106260cdaf3235eae127aea987424f6ab5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:10:15 +1100 Subject: [PATCH 243/293] ROMFS: update from upstream --- .../init.d/1000_rc_fw_easystar.hil | 37 ++- .../px4fmu_common/init.d/10015_tbs_discovery | 23 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 13 +- .../init.d/10017_steadidrone_qu4d | 17 +- .../px4fmu_common/init.d/10018_tbs_endurance | 8 +- .../px4fmu_common/init.d/10019_sk450_deadcat | 15 +- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 6 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 6 +- .../init.d/1004_rc_fw_Rascal110.hil | 6 +- .../init.d/1005_rc_fw_Malolo1.hil | 6 +- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 17 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 6 +- .../init.d/13001_caipirinha_vtol | 8 +- ROMFS/px4fmu_common/init.d/13002_firefly6 | 22 +- ROMFS/px4fmu_common/init.d/14001_tri_y_yaw+ | 7 +- ROMFS/px4fmu_common/init.d/14002_tri_y_yaw- | 7 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 6 +- ROMFS/px4fmu_common/init.d/2101_hk_bixler | 5 - ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 17 + .../px4fmu_common/init.d/2103_skyhunter_1800 | 15 + ROMFS/px4fmu_common/init.d/3030_io_camflyer | 39 +++ ROMFS/px4fmu_common/init.d/3031_phantom | 18 +- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 38 ++- ROMFS/px4fmu_common/init.d/3033_wingwing | 34 +- ROMFS/px4fmu_common/init.d/3034_fx79 | 14 +- ROMFS/px4fmu_common/init.d/3035_viper | 7 +- .../px4fmu_common/init.d/3100_tbs_caipirinha | 51 +-- ROMFS/px4fmu_common/init.d/4001_quad_x | 10 +- ROMFS/px4fmu_common/init.d/4008_ardrone | 6 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 23 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 30 +- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 19 +- ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb | 8 +- .../init.d/50001_axialracing_ax10 | 5 +- ROMFS/px4fmu_common/init.d/5001_quad_+ | 6 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 10 +- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 10 +- ROMFS/px4fmu_common/init.d/8001_octo_x | 10 +- ROMFS/px4fmu_common/init.d/9001_octo_+ | 10 +- ROMFS/px4fmu_common/init.d/rc.autostart | 296 ------------------ ROMFS/px4fmu_common/init.d/rc.fw_defaults | 18 +- ROMFS/px4fmu_common/init.d/rc.interface | 30 +- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 19 +- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 71 ++--- ROMFS/px4fmu_common/init.d/rc.sensors | 75 +++-- ROMFS/px4fmu_common/init.d/rc.uavcan | 19 +- ROMFS/px4fmu_common/init.d/rc.usb | 18 +- ROMFS/px4fmu_common/init.d/rc.vtol_apps | 8 +- ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 5 +- ROMFS/px4fmu_common/init.d/rcS | 181 +++++++++-- ROMFS/px4fmu_common/mixers/FX79.main.mix | 24 +- ROMFS/px4fmu_common/mixers/README.md | 24 +- ROMFS/px4fmu_common/mixers/firefly6.aux.mix | 6 +- ROMFS/px4fmu_test/init.d/rcS | 6 +- 55 files changed, 731 insertions(+), 666 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/2101_hk_bixler delete mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart 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 40b9ed8df5ed..7938c47bae64 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,10 +1,43 @@ #!nsh # -# HILStar -# +# @name HILStar (XPlane) +# +# @type Simulation +# +# @output MAIN1 aileron +# @output MAIN2 elevator +# @output MAIN3 rudder +# @output MAIN4 throttle +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.fw_defaults +if [ $AUTOCNF == yes ] +then + param set BAT_N_CELLS 3 + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_MIN 12 + param set FW_AIRSPD_TRIM 14 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_PR_I 0.1 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.2 + param set FW_RR_FF 0.6 + param set FW_RR_I 0.1 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.3 +fi + set HIL yes set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 0dd4837644e6..97beb6831aa9 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,21 +1,22 @@ #!nsh # -# Team Blacksheep Discovery Quadcopter +# @name Team Blacksheep Discovery # -# Anton Babushkin , Simon Wilks +# @type Quadrotor Wide +# +# @maintainer Anton Babushkin , Simon Wilks # sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then - # TODO review MC_YAWRATE_I - param set MC_ROLL_P 8.0 - param set MC_ROLLRATE_P 0.07 + param set MC_ROLL_P 6.5 + param set MC_ROLLRATE_P 0.1 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.0017 - param set MC_PITCH_P 8.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCH_P 6.5 + param set MC_PITCHRATE_P 0.14 param set MC_PITCHRATE_I 0.1 param set MC_PITCHRATE_D 0.0025 param set MC_YAW_P 2.8 @@ -27,11 +28,3 @@ fi set MIXER quad_w set PWM_OUT 1234 -set PWM_MIN 1200 - -set MIXER_AUX pass -set PWM_AUX_RATE 50 -set PWM_AUX_OUT 1234 -set PWM_AUX_DISARMED 1000 -set PWM_AUX_MIN 1000 -set PWM_AUX_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 68428be6f659..30af86730e8e 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,8 +1,10 @@ #!nsh # -# 3DR Iris Quadcopter +# @name 3DR Iris Quadrotor # -# Anton Babushkin +# @type Quadrotor Wide +# +# @maintainer Anton Babushkin # sh /etc/init.d/rc.mc_defaults @@ -11,11 +13,11 @@ if [ $AUTOCNF == yes ] then # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_P 0.15 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_P 0.15 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 2.5 @@ -30,6 +32,3 @@ fi set MIXER quad_w set PWM_OUT 1234 - -set PWM_MIN 1200 -set PWM_MAX 1950 diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 8ab65be7b940..7accc4f6e902 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -1,34 +1,33 @@ #!nsh # -# Steadidrone QU4D +# @name Steadidrone QU4D # -# Thomas Gubler +# @type Quadrotor Wide +# +# @maintainer Thomas Gubler # sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then - # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.13 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_P 0.19 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 0.5 + param set MC_YAW_P 4.0 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.5 param set BAT_N_CELLS 4 fi set MIXER quad_w -set PWM_MIN 1210 -set PWM_MAX 2100 - set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance index e83a864d8d4f..e26e2a81053b 100644 --- a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance +++ b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance @@ -1,10 +1,10 @@ #!nsh # -# Team Blacksheep Discovery Long Range Quadcopter -# -# Setup: 15 x 5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors +# @name Team Blacksheep Discovery Endurance # -# Simon Wilks +# @type Quadrotor Wide +# +# @maintainer Simon Wilks # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat index c6861c2d452c..0b1e3b2dfa91 100644 --- a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -1,8 +1,10 @@ #!nsh # -# HobbyKing SK450 DeadCat modification +# @name HobbyKing SK450 DeadCat modification # -# Anton Matosov +# @type Quadrotor Wide +# +# @maintainer Anton Matosov # sh /etc/init.d/rc.mc_defaults @@ -10,13 +12,13 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then param set MC_ROLL_P 6.0 - param set MC_ROLLRATE_P 0.04 - param set MC_ROLLRATE_I 0.1 + param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_I 0.03 param set MC_ROLLRATE_D 0.0015 param set MC_PITCH_P 6.0 - param set MC_PITCHRATE_P 0.08 - param set MC_PITCHRATE_I 0.2 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.03 param set MC_PITCHRATE_D 0.0015 param set MC_YAW_P 2.8 @@ -28,7 +30,6 @@ fi set MIXER sk450_deadcat set PWM_OUT 1234 -set PWM_MIN 1050 set PWM_AUX_OUT 1234 # set PWM_AUX_MIN 900 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 03b6b30d27f4..e1f2cdfe8dad 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -1,8 +1,10 @@ #!nsh # -# HIL Quadcopter X +# @name HIL Quadcopter X # -# Anton Babushkin +# @type Simulation +# +# @maintainer 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 7e651216d0de..7aa888c76383 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -1,8 +1,10 @@ #!nsh # -# HIL Quadcopter + +# @name HIL Quadcopter + # -# Anton Babushkin +# @type Simulation +# +# @maintainer 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 0909208478fc..57bcd24d021d 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -1,8 +1,10 @@ #!nsh # -# HIL Rascal 110 (Flightgear) +# @name HIL Rascal 110 (Flightgear) # -# Thomas Gubler +# @type Simulation +# +# @maintainer Thomas Gubler # sh /etc/init.d/rc.fw_defaults 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 15e5cf21d82f..5e0b6cd74618 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -1,8 +1,10 @@ #!nsh # -# HIL Malolo 1 (Flightgear) +# @name HIL Malolo 1 (Flightgear) # -# Thomas Gubler +# @type Simulation +# +# @maintainer 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 0bb8cb52eefa..8c9c28fe3795 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -1,8 +1,21 @@ #!nsh # -# Generic 10" Hexa coaxial geometry +# @name Generic Hexa coaxial geometry # -# Lorenz Meier +# @type Hexarotor Coaxial +# +# @output MAIN1 front right top, CW; angle:60; direction:CW +# @output MAIN2 front right bottom, CCW; angle:60; direction:CCW +# @output MAIN3 back top, CW; angle:180; direction:CW +# @output MAIN4 back bottom, CCW; angle:180; direction:CCW +# @output MAIN5 front left top, CW; angle:-60; direction:CW +# @output MAIN6 front left bottom, CCW;angle:-60; direction:CCW +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 16e86fd5fa13..431d883155b0 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -1,8 +1,10 @@ #!nsh # -# Generic 10" Octo coaxial geometry +# @name Generic 10" Octo coaxial geometry # -# Lorenz Meier +# @type Octorotor Coaxial +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 5c0004149268..a9e7ff35b251 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -1,7 +1,10 @@ +#!nsh # -# Generic configuration file for caipirinha VTOL version +# @name Duorotor Tailsitter # -# Roman Bapst +# @type VTOL Tailsitter +# +# @maintainer Roman Bapst # sh /etc/init.d/rc.vtol_defaults @@ -13,3 +16,4 @@ set PWM_MAX 2000 set PWM_RATE 400 param set VT_MOT_COUNT 2 param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index ed90dabf41d8..d840f6c7e0be 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -1,8 +1,10 @@ #!nsh # -# Generic configuration file for BirdsEyeView Aerobotics FireFly6 +# @name BirdsEyeView Aerobotics FireFly6 # -# Roman Bapst +# @type VTOL Tiltrotor +# +# @maintainer Roman Bapst # sh /etc/init.d/rc.vtol_defaults @@ -10,21 +12,25 @@ sh /etc/init.d/rc.vtol_defaults if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_P 0.17 param set MC_ROLLRATE_I 0.002 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.004 param set MC_ROLLRATE_FF 0.0 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_P 0.14 param set MC_PITCHRATE_I 0.002 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.004 param set MC_PITCHRATE_FF 0.0 - param set MC_YAW_P 2.8 + param set MC_YAW_P 3.8 param set MC_YAW_FF 0.5 param set MC_YAWRATE_P 0.22 param set MC_YAWRATE_I 0.02 param set MC_YAWRATE_D 0.0 param set MC_YAWRATE_FF 0.0 + + param set VT_TILT_MC 0.08 + param set VT_TILT_TRANS 0.5 + param set VT_TILT_FW 0.9 fi set MIXER firefly6 @@ -40,4 +46,6 @@ set PWM_AUX_MAX 2000 set MAV_TYPE 21 param set VT_MOT_COUNT 6 +param set VT_FW_MOT_OFF 23 param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 1 diff --git a/ROMFS/px4fmu_common/init.d/14001_tri_y_yaw+ b/ROMFS/px4fmu_common/init.d/14001_tri_y_yaw+ index 3bb68ecf3f16..b4c7885dab47 100644 --- a/ROMFS/px4fmu_common/init.d/14001_tri_y_yaw+ +++ b/ROMFS/px4fmu_common/init.d/14001_tri_y_yaw+ @@ -1,9 +1,10 @@ #!nsh # -# Generic Tricopter Y Geometry -# Yaw Servo +Output ==> +Yaw Vehicle Rotation +# @name Generic Tricopter Y+ Geometry # -# Trent Lukaczyk +# @type Tricopter Y+ +# +# @maintainer Trent Lukaczyk # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/14002_tri_y_yaw- b/ROMFS/px4fmu_common/init.d/14002_tri_y_yaw- index fd4d47647638..7fa0af009a26 100644 --- a/ROMFS/px4fmu_common/init.d/14002_tri_y_yaw- +++ b/ROMFS/px4fmu_common/init.d/14002_tri_y_yaw- @@ -1,9 +1,10 @@ #!nsh # -# Generic Tricopter Y Geometry -# Yaw Servo +Output ==> -Yaw Vehicle Rotation +# @name Generic Tricopter Y- Geometry # -# Trent Lukaczyk +# @type Tricopter Y- +# +# @maintainer Trent Lukaczyk # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 3ab2ac3d1a0d..12c12aec92fc 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,6 +1,10 @@ #!nsh # -# MPX EasyStar Plane +# @name Multiplex Easystar +# +# @type Standard Plane +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler deleted file mode 100644 index 05ee57ffa05b..000000000000 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ /dev/null @@ -1,5 +0,0 @@ -#!nsh - -sh /etc/init.d/rc.fw_defaults - -set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index 906f4b1ccf07..d2347c98c818 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -1,4 +1,21 @@ #!nsh +# +# @name Skywalker (3DR Aero) +# +# @type Standard Plane +# +# @output MAIN1 aileron +# @output MAIN2 elevator +# @output MAIN4 rudder +# @output MAIN3 throttle +# @output MAIN5 flaps +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier +# sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 index 2433ab4f40bd..133ffe57220c 100644 --- a/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 +++ b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 @@ -1,4 +1,19 @@ #!nsh +# +# @name Skyhunter 1800 +# +# @type Standard Plane +# +# @output MAIN1 aileron +# @output MAIN2 elevator +# @output MAIN4 throttle +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier +# sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 18867832499c..a15d0ac6d072 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -1,7 +1,46 @@ #!nsh +# +# @name IO Camflyer +# +# @url https://pixhawk.org/platforms/planes/bormatec_camflyer_q +# +# @type Flying Wing +# +# @output MAIN1 left aileron +# @output MAIN2 right aileron +# @output MAIN4 throttle +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Simon Wilks +# sh /etc/init.d/rc.fw_defaults +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MAX 15 + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 13 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.08 + param set FW_RR_FF 0.6 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.04 +fi + set MIXER Q # Provide ESC a constant 1000 us pulse while disarmed set PWM_OUT 4 diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index c7dd1dfc5c3c..84d9a9f39e2c 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -1,8 +1,20 @@ #!nsh # -# Phantom FPV Flying Wing +# @name Phantom FPV Flying Wing # -# Simon Wilks +# @url https://pixhawk.org/platforms/planes/z-84_wing_wing +# +# @type Flying Wing +# +# @output MAIN1 left aileron +# @output MAIN2 right aileron +# @output MAIN4 throttle +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Simon Wilks # sh /etc/init.d/rc.fw_defaults @@ -16,14 +28,12 @@ then param set FW_L1_DAMPING 0.75 param set FW_L1_PERIOD 15 param set FW_PR_FF 0.2 - param set FW_PR_I 0.005 param set FW_PR_IMAX 0.2 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_ROLLFF 1 param set FW_RR_FF 0.5 - param set FW_RR_I 0.02 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.08 param set FW_R_LIM 50 diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 3d464a4ae97d..3dedf8170dfa 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -1,8 +1,20 @@ #!nsh # -# Skywalker X5 Flying Wing +# @name Skywalker X5 Flying Wing # -# Thomas Gubler , Julian Oes +# @url https://pixhawk.org/platforms/planes/skywalker_x5 +# +# @type Flying Wing +# +# @output MAIN1 left aileron +# @output MAIN2 right aileron +# @output MAIN4 throttle +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Thomas Gubler , Julian Oes # sh /etc/init.d/rc.fw_defaults @@ -14,18 +26,16 @@ then param set FW_AIRSPD_MAX 40 param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.3 - param set FW_PR_I 0 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.03 - param set FW_P_ROLLFF 1 - param set FW_RR_FF 0.3 - param set FW_RR_I 0 - param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.03 - param set FW_R_LIM 60 - param set FW_R_RMAX 0 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_RR_FF 0.6 + param set FW_RR_P 0.04 fi set MIXER X5 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index add905b11570..3a0aa2815052 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -1,8 +1,20 @@ #!nsh # -# Wing Wing (aka Z-84) Flying Wing +# @name Wing Wing (aka Z-84) Flying Wing # -# Simon Wilks +# @url https://pixhawk.org/platforms/planes/z-84_wing_wing +# +# @type Flying Wing +# +# @output MAIN1 left aileron +# @output MAIN2 right aileron +# @output MAIN4 throttle +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.fw_defaults @@ -23,25 +35,13 @@ then param set FW_LND_TLALT 5 param set FW_THR_LND_MAX 0 param set FW_PR_FF 0.35 - param set FW_PR_I 0.005 - param set FW_PR_IMAX 0.4 - param set FW_PR_P 0.08 param set FW_RR_FF 0.6 - param set FW_RR_I 0.005 - param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 - param set MT_TKF_PIT_MAX 30.0 - param set MT_ACC_D 0.2 - param set MT_ACC_P 0.6 - param set MT_A_LP 0.5 - param set MT_PIT_OFF 0.1 - param set MT_PIT_I 0.1 - param set MT_THR_OFF 0.65 - param set MT_THR_I 0.35 - param set MT_THR_P 0.2 - param set MT_THR_FF 1.5 fi +# Configure this as plane +set MAV_TYPE 1 +# Set mixer set MIXER wingwing # Provide ESC a constant 1000 us pulse set PWM_OUT 4 diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index d46147ede56c..353da2ffc498 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -1,10 +1,20 @@ #!nsh # -# FX-79 Buffalo Flying Wing +# @name FX-79 Buffalo Flying Wing # -# Simon Wilks +# @type Flying Wing +# +# @maintainer Simon Wilks # sh /etc/init.d/rc.fw_defaults +if [ $AUTOCNF == yes ] +then + param set NAV_LOITER_RAD 150 + param set FW_AIRSPD_MAX 30 + param set FW_AIRSPD_MIN 13 + param set FW_AIRSPD_TRIM 15 +fi + set MIXER FX79 diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index 0f5f5502aa59..c216abf7b25e 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -1,8 +1,11 @@ +# #!nsh # -# Viper +# @name Viper +# +# @type Flying Wing # -# Simon Wilks +# @maintainer 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 9e1c1c170a04..5aa004db1b5d 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -1,34 +1,45 @@ #!nsh # -# TBS Caipirinha +# @name TBS Caipirinha # -# Thomas Gubler +# @type Flying Wing +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.fw_defaults if [ $AUTOCNF == yes ] then - - # TODO: these are the X5 default parameters, update them to the caipi - - param set FW_AIRSPD_MIN 15 - param set FW_AIRSPD_TRIM 20 - param set FW_AIRSPD_MAX 40 + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_MIN 12 + param set FW_AIRSPD_TRIM 16 param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.3 - param set FW_PR_I 0 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.03 - param set FW_P_ROLLFF 0 - param set FW_RR_FF 0.3 - param set FW_RR_I 0 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.08 + param set FW_RR_FF 0.6 param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.03 - param set FW_R_LIM 60 - param set FW_R_RMAX 0 + param set FW_RR_P 0.04 + param set SYS_COMPANION 157600 + param set PWM_MAIN_REV0 1 + param set PWM_MAIN_REV1 1 + param set PWM_DISARMED 0 + param set PWM_MIN 900 + param set PWM_MAX 2100 fi -set MIXER Q +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX + +set MIXER caipi +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index e0538160f09e..8f7c8da3567d 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -1,8 +1,14 @@ #!nsh # -# Generic 10" Quad X geometry +# @name Generic Quadrotor X config # -# Lorenz Meier +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer 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 c3358ef4c8f0..9d3e4427bedc 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -1,6 +1,10 @@ #!nsh # -# ARDrone +# @name AR.Drone Frame +# +# @type Quadrotor x +# +# @maintainer Lorenz Meier # 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 d07e926a7928..aea5292a24d5 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,8 +1,14 @@ #!nsh # -# DJI Flame Wheel F330 +# @name DJI Flame Wheel F330 # -# Anton Babushkin +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier # sh /etc/init.d/4001_quad_x @@ -10,18 +16,23 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.15 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.15 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 + # DJI ESCs do not support calibration and need a higher min + param set PWM_MIN 1230 fi -set PWM_MIN 1200 -set PWM_MAX 1950 +# Transitional support: ensure suitable PWM min/max param values +if param compare PWM_MIN 1075 +then + param set PWM_MIN 1230 +fi diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 9b3954be6fcf..5e7ba1bda539 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,28 +1,38 @@ #!nsh # -# DJI Flame Wheel F450 +# @name DJI Flame Wheel F450 # -# Lorenz Meier +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier # sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then - # TODO REVIEW param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.15 param set MC_ROLLRATE_I 0.05 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.01 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.15 param set MC_PITCHRATE_I 0.05 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.01 param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 + # DJI ESCs do not support calibration and need a higher min + param set PWM_MIN 1230 fi -set PWM_MIN 1230 -set PWM_MAX 1950 +# Transitional support: ensure suitable PWM min/max param values +if param compare PWM_MIN 1075 +then + param set PWM_MIN 1230 +fi diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 05b435513869..1ffffea22bc0 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -1,26 +1,27 @@ #!nsh # -# F450-sized quadrotor with CAN +# @name F450-sized quadrotor with CAN # -# Pavel Kirienko +# @type Quadrotor x +# +# @maintainer Pavel Kirienko # sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then - # TODO REVIEW param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.16 param set MC_ROLLRATE_I 0.05 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.01 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.16 param set MC_PITCHRATE_I 0.05 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.01 param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index aec0c62f8fb5..a6a31a30a9a2 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -1,11 +1,11 @@ #!nsh # -# Hobbyking Micro Integrated PCB Quadcopter -# with SimonK ESC firmware and Mystery A1510 motors +# @name Hobbyking Micro PCB # -# Thomas Gubler +# @type Quadrotor x +# +# @maintainer Thomas Gubler # -echo "HK Micro PCB Quad" sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 b/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 index a36bac5cc804..b3e7f0978ea2 100644 --- a/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 +++ b/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 @@ -1,9 +1,10 @@ #!nsh # -# loading default values for the axialracing ax10 +# @name Axial Racing AX10 +# +# @type Rover # -#load some defaults e.g. PWM values sh /etc/init.d/rc.axialracing_ax10_defaults #choose a mixer, for rover control we need a plain passthrough to the servos diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index e51f9cf89d82..8c5eed28cd2a 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -1,8 +1,10 @@ #!nsh # -# Generic 10" Quad + geometry +# @name Generic 10" Quad + geometry # -# Anton Babushkin +# @type Quadrotor + +# +# @maintainer 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 7a6dda648a21..584c4a381ce6 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -1,8 +1,14 @@ #!nsh # -# Generic 10" Hexa X geometry +# @name Generic Hexarotor x geometry # -# Anton Babushkin +# @type Hexarotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index dd9589d614f4..11d732471a4b 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -1,8 +1,14 @@ #!nsh # -# Generic 10" Hexa + geometry +# @name Generic Hexarotor + geometry # -# Anton Babushkin +# @type Hexarotor + +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 7cbb3ddfcd97..272e09355049 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -1,8 +1,14 @@ #!nsh # -# Generic 10" Octo X geometry +# @name Generic Octocopter X geometry # -# Anton Babushkin +# @type Octorotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer 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 5d608d593c63..d0db57cf1794 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -1,8 +1,14 @@ #!nsh # -# Generic 10" Octo + geometry +# @name Generic Octocopter + geometry # -# Anton Babushkin +# @type Octorotor + +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer 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 deleted file mode 100644 index de81795b44e8..000000000000 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ /dev/null @@ -1,296 +0,0 @@ -# -# SYS_AUTOSTART = 0 means no autostart (default) -# -# AUTOSTART PARTITION: -# 0 .. 999 Reserved (historical) -# 1000 .. 1999 Simulation setups -# 2000 .. 2999 Standard planes -# 3000 .. 3999 Flying wing -# 4000 .. 4999 Quad X -# 5000 .. 5999 Quad + -# 6000 .. 6999 Hexa X -# 7000 .. 7999 Hexa + -# 8000 .. 8999 Octo X -# 9000 .. 9999 Octo + -# 10000 .. 10999 Wide arm / H frame -# 11000 .. 11999 Hexa Cox -# 12000 .. 12999 Octo Cox -# 13000 .. 13999 VTOL -# 14000 .. 14999 Tri Y - -# -# Simulation -# - -if param compare SYS_AUTOSTART 901 -then - sh /etc/init.d/901_bottle_drop_test.hil - set MODE custom -fi - -if param compare SYS_AUTOSTART 1000 -then - sh /etc/init.d/1000_rc_fw_easystar.hil -fi - -if param compare SYS_AUTOSTART 1001 -then - sh /etc/init.d/1001_rc_quad_x.hil -fi - -if param compare SYS_AUTOSTART 1003 -then - sh /etc/init.d/1003_rc_quad_+.hil -fi - -if param compare SYS_AUTOSTART 1004 -then - sh /etc/init.d/1004_rc_fw_Rascal110.hil -fi - -if param compare SYS_AUTOSTART 1005 -then - sh /etc/init.d/1005_rc_fw_Malolo1.hil -fi - -# -# Plane -# - -if param compare SYS_AUTOSTART 2100 100 -then - sh /etc/init.d/2100_mpx_easystar - set MODE custom -fi - -if param compare SYS_AUTOSTART 2101 101 -then - sh /etc/init.d/2101_hk_bixler - set MODE custom -fi - -if param compare SYS_AUTOSTART 2102 102 -then - sh /etc/init.d/2102_3dr_skywalker - set MODE custom -fi - -if param compare SYS_AUTOSTART 2103 103 -then - sh /etc/init.d/2103_skyhunter_1800 - set MODE custom -fi - -# -# Flying wing -# - -if param compare SYS_AUTOSTART 3030 30 -then - sh /etc/init.d/3030_io_camflyer -fi - -if param compare SYS_AUTOSTART 3031 31 -then - sh /etc/init.d/3031_phantom -fi - -if param compare SYS_AUTOSTART 3032 32 -then - sh /etc/init.d/3032_skywalker_x5 -fi - -if param compare SYS_AUTOSTART 3033 33 -then - sh /etc/init.d/3033_wingwing -fi - -if param compare SYS_AUTOSTART 3034 34 -then - sh /etc/init.d/3034_fx79 -fi - -if param compare SYS_AUTOSTART 3035 35 -then - sh /etc/init.d/3035_viper -fi - -if param compare SYS_AUTOSTART 3100 -then - sh /etc/init.d/3100_tbs_caipirinha -fi - -# -# Quad X -# - -if param compare SYS_AUTOSTART 4001 -then - sh /etc/init.d/4001_quad_x -fi - -# -# ARDrone -# - -if param compare SYS_AUTOSTART 4008 8 -then - sh /etc/init.d/4008_ardrone -fi - -if param compare SYS_AUTOSTART 4010 10 -then - sh /etc/init.d/4010_dji_f330 -fi - -if param compare SYS_AUTOSTART 4011 11 -then - sh /etc/init.d/4011_dji_f450 -fi - -if param compare SYS_AUTOSTART 4012 -then - sh /etc/init.d/4012_quad_x_can -fi - -if param compare SYS_AUTOSTART 4020 -then - sh /etc/init.d/4020_hk_micro_pcb -fi - -# -# Quad + -# - -if param compare SYS_AUTOSTART 5001 -then - sh /etc/init.d/5001_quad_+ -fi - -# -# Hexa X -# - -if param compare SYS_AUTOSTART 6001 -then - sh /etc/init.d/6001_hexa_x -fi - -# -# Hexa + -# - -if param compare SYS_AUTOSTART 7001 -then - sh /etc/init.d/7001_hexa_+ -fi - -# -# Octo X -# - -if param compare SYS_AUTOSTART 8001 -then - sh /etc/init.d/8001_octo_x -fi - -# -# Octo + -# - -if param compare SYS_AUTOSTART 9001 -then - sh /etc/init.d/9001_octo_+ -fi - -# -# Wide arm / H frame -# - -if param compare SYS_AUTOSTART 10015 15 -then - sh /etc/init.d/10015_tbs_discovery -fi - -if param compare SYS_AUTOSTART 10016 16 -then - sh /etc/init.d/10016_3dr_iris -fi - -if param compare SYS_AUTOSTART 10017 -then - sh /etc/init.d/10017_steadidrone_qu4d -fi - -if param compare SYS_AUTOSTART 10018 18 -then - sh /etc/init.d/10018_tbs_endurance -fi - -if param compare SYS_AUTOSTART 10019 -then - sh /etc/init.d/10019_sk450_deadcat -fi - -# -# Hexa Coaxial -# - -if param compare SYS_AUTOSTART 11001 -then - sh /etc/init.d/11001_hexa_cox -fi - -# -# Octo Coaxial -# - -if param compare SYS_AUTOSTART 12001 -then - sh /etc/init.d/12001_octo_cox -fi - -# 13000 is historically reserved for the quadshot - -# -# VTOL Caipririnha (Tailsitter) -# -if param compare SYS_AUTOSTART 13001 -then - sh /etc/init.d/13001_caipirinha_vtol -fi - -# -# VTOL BirdsEyeView FireFly x6 (Tilt-Rotor) -# -if param compare SYS_AUTOSTART 13002 -then - sh /etc/init.d/13002_firefly6 -fi - -# -# TriCopter Y Yaw+ -# -if param compare SYS_AUTOSTART 14001 -then - sh /etc/init.d/14001_tri_y_yaw+ -fi - -# -# TriCopter Y Yaw- -# -if param compare SYS_AUTOSTART 14002 -then - sh /etc/init.d/14002_tri_y_yaw- -fi - - - -# -# Ground Rover AxialRacing AX10 -# -if param compare SYS_AUTOSTART 50001 -then - sh /etc/init.d/50001_axialracing_ax10 -fi - diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 156711c26d3a..c0bd5b1c85eb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -11,15 +11,17 @@ then param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 param set NAV_ACC_RAD 50 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 param set PE_VELNE_NOISE 0.3 - param set PE_VELD_NOISE 0.5 + param set PE_VELD_NOISE 0.35 param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 0.5 - param set PE_GBIAS_PNOISE 0.000001 - param set PE_ABIAS_PNOISE 0.0002 + param set PE_POSD_NOISE 1.0 fi + +# This is the gimbal pass mixer +set MIXER_AUX pass +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1500 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 71b670888e43..0c471808a825 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -97,8 +97,13 @@ then fi fi -# check if should load secondary mixer -if [ $MIXER_AUX != none ] +# This is a FMUv2+ thing +if ver hwcmp PX4FMU_V1 +then + set MIXER_AUX none +fi + +if [ $MIXER_AUX != none -a $AUX_MODE != none ] then # # Load aux mixer @@ -118,20 +123,28 @@ then fi fi - if [ $MIXER_AUX_FILE != none -a $FMU_MODE == pwm ] + if [ $MIXER_AUX_FILE != none ] then - if fmu mode_pwm + if fmu mode_$AUX_MODE then - if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE + if [ -e $OUTPUT_AUX_DEV ] then - echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV" + if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE + then + echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV" + else + echo "[i] Error loading mixer: $MIXER_AUX_FILE" + echo "ERROR: Could not load mixer: $MIXER_AUX_FILE" >> $LOG_FILE + fi else - echo "[i] Error loading mixer: $MIXER_AUX_FILE" - echo "ERROR: Could not load mixer: $MIXER_AUX_FILE" >> $LOG_FILE + set PWM_AUX_OUT none + set FAILSAFE_AUX none fi else echo "ERROR: Could not start: fmu mode_pwm" >> $LOG_FILE tone_alarm $TUNE_ERR + set PWM_AUX_OUT none + set FAILSAFE_AUX none fi if [ $PWM_AUX_OUT != none ] @@ -169,3 +182,4 @@ then fi fi unset OUTPUT_DEV +unset OUTPUT_AUX_DEV diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 6bfbfea396be..5d374745a1ae 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -11,7 +11,7 @@ then then fi else - if sdlog2 start -r 100 -a -b 22 -t + if sdlog2 start -r 100 -a -b 12 -t then fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 914807889fc2..4c6cdbd7cde7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -9,21 +9,16 @@ # filter by setting INAV_ENABLED = 0 if param compare INAV_ENABLED 1 then - # The system is defaulting to EKF_ATT_ENABLED = 1 - # and uses the older EKF filter. However users can - # enable the new quaternion based complimentary - # filter by setting EKF_ATT_ENABLED = 0. - # Note that on FMUv1, the EKF att estimator is not - # available and the Q estimator runs instead. - if param compare EKF_ATT_ENABLED 1 + attitude_estimator_q start + position_estimator_inav start +else + if param compare LPE_ENABLED 1 then - attitude_estimator_ekf start - else attitude_estimator_q start + local_position_estimator start + else + ekf_att_pos_estimator start fi - position_estimator_inav start -else - ekf_att_pos_estimator start fi if mc_att_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 1f34282aec7a..108012161844 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -4,51 +4,44 @@ set VEHICLE_TYPE mc if [ $AUTOCNF == yes ] then - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.003 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.003 - param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAW_FF 0.5 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILTMAX_AIR 45.0 - param set MPC_TILTMAX_LND 15.0 - param set MPC_LAND_SPEED 1.0 - param set PE_VELNE_NOISE 0.5 - param set PE_VELD_NOISE 0.7 + param set PE_VELD_NOISE 0.35 param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 1.0 - param set PE_GBIAS_PNOISE 0.000001 - param set PE_ABIAS_PNOISE 0.0001 + param set PE_POSD_NOISE 1.25 param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 param set RTL_DESCEND_ALT 10.0 + param set PWM_DISARMED 900 + param set PWM_MIN 1075 + param set PWM_MAX 1950 +fi + +# Transitional support: ensure suitable PWM min/max param values +if param compare PWM_MIN 1000 +then + param set PWM_MIN 1075 +fi +if param compare PWM_MAX 2000 +then + param set PWM_MAX 1950 +fi +if param compare PWM_DISARMED 0 +then + param set PWM_DISARMED 900 fi +# set environment variables (!= parameters) set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1075 -set PWM_MAX 2000 +# tell the mixer to use parameters for these instead +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX + +# This is the gimbal pass mixer +set MIXER_AUX pass +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1500 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 536cfca91aca..a7bc6b08158c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -67,32 +67,55 @@ then fi fi else - # FMUv1 - if mpu6000 start + if ver hwcmp PX4FMU_V4 then - fi + # External I2C bus + if hmc5883 -C -T -X start + then + fi - if l3gd20 start - then - fi + # Internal SPI bus is rotated 90 deg yaw + if hmc5883 -C -T -S -R 2 start + then + fi - # MAG selection - if param compare SENS_EXT_MAG 2 - then - if hmc5883 -C -I start + # Internal SPI bus ICM-20608-G is rotated 90 deg yaw + if mpu6000 -R 2 -T 20608 start + then + fi + + # Internal SPI bus mpu9250 is rotated 90 deg yaw + if mpu9250 -R 2 start then fi else - # Use only external as primary - if param compare SENS_EXT_MAG 1 + # FMUv1 + if mpu6000 start + then + fi + + if l3gd20 start + then + fi + + # MAG selection + if param compare SENS_EXT_MAG 2 then - if hmc5883 -C -X start + if hmc5883 -C -I start then fi else - # auto-detect the primary, prefer external - if hmc5883 start + # Use only external as primary + if param compare SENS_EXT_MAG 1 then + if hmc5883 -C -X start + then + fi + else + # auto-detect the primary, prefer external + if hmc5883 start + then + fi fi fi fi @@ -110,24 +133,8 @@ else fi fi -# Check for flow sensor -if px4flow start +# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire) +usleep 20000 +if sensors start then fi - -if param compare SENS_EN_LL40LS 1 -then - if pwm_input start - then - fi - - if ll40ls start pwm - then - fi -fi - -# -# Start sensors -# -sensors start - diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 08ba86d78905..ebecea2de426 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -3,16 +3,27 @@ # UAVCAN initialization script. # -if param compare UAVCAN_ENABLE 1 +# +# Starting stuff according to UAVCAN_ENABLE value +# +if param greater UAVCAN_ENABLE 0 then if uavcan start then - # First sensor publisher to initialize takes lowest instance ID - # This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs - sleep 1 echo "[i] UAVCAN started" else echo "[i] ERROR: Could not start UAVCAN" tone_alarm $TUNE_ERR fi fi + +if param greater UAVCAN_ENABLE 1 +then + if uavcan start fw + then + echo "[i] UAVCAN servers started" + else + echo "[i] ERROR: Could not start UAVCAN servers" + tone_alarm $TUNE_ERR + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 4c2119aa2f08..4a157a683f5d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,23 +3,7 @@ # USB MAVLink start # -mavlink start -r 80000 -d /dev/ttyACM0 -x -# Enable a number of interesting streams we want via USB -mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 -mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 -mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 -mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 -mavlink stream -d /dev/ttyACM0 -s DISTANCE_SENSOR -r 10 -mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 -mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 -mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30 -mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 -mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 -mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 -mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 -mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5 -mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 20 -mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20 +mavlink start -r 800000 -d /dev/ttyACM0 -m config -x # Exit shell to make it available to MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 23ade6d78b52..7d0c153f096f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -4,7 +4,7 @@ # att & pos estimator, att & pos control. # -attitude_estimator_ekf start +attitude_estimator_q start #ekf_att_pos_estimator start position_estimator_inav start @@ -13,3 +13,9 @@ mc_att_control start mc_pos_control start fw_att_control start fw_pos_control_l1 start + +# +# Start Land Detector +# Multicopter for now until we have something for VTOL +# +land_detector start multicopter diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 844d540bf03c..1b7b6528cf6f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -34,10 +34,9 @@ then param set FW_RR_P 0.02 param set PE_VELNE_NOISE 0.5 - param set PE_VELD_NOISE 0.7 + param set PE_VELD_NOISE 0.3 param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 1.0 - param set PE_GBIAS_PNOISE 0.000001 + param set PE_POSD_NOISE 1.25 param set PE_ABIAS_PNOISE 0.0001 fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2dfe3d8e3020..44b85c35bc13 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -89,6 +89,70 @@ then fi fi + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_THROTTLE 0 + then + # So this is a default setup, + # now lets find out if channel 3 + # is calibrated + if param compare RC3_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_THROTTLE 3 + fi + fi + + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_ROLL 0 + then + # So this is a default setup, + # now lets find out if channel 1 + # is calibrated + if param compare RC1_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_ROLL 1 + fi + fi + + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_PITCH 0 + then + # So this is a default setup, + # now lets find out if channel 2 + # is calibrated + if param compare RC2_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_PITCH 2 + fi + fi + + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_YAW 0 + then + # So this is a default setup, + # now lets find out if channel 4 + # is calibrated + if param compare RC4_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_YAW 4 + fi + fi + # # Start system state indicator # @@ -101,9 +165,10 @@ then fi fi - if pca8574 start - then - fi + # Currently unused, but might be useful down the road + #if pca8574 start + #then + #fi # # Set default values @@ -126,6 +191,7 @@ then set FAILSAFE_AUX none set MK_MODE none set FMU_MODE pwm + set AUX_MODE pwm set MAVLINK_F default set EXIT_ON_END no set MAV_TYPE none @@ -151,7 +217,12 @@ then # if param compare SYS_USE_IO 1 then - set USE_IO yes + if ver hwcmp PX4FMU_V4 + then + set USE_IO no + else + set USE_IO yes + fi else set USE_IO no fi @@ -194,11 +265,11 @@ then # # Check if PX4IO present and update firmware if needed # - if [ -f /etc/extras/px4io-v2_default.bin ] + if [ -f /etc/extras/px4io-v2.bin ] then - set IO_FILE /etc/extras/px4io-v2_default.bin + set IO_FILE /etc/extras/px4io-v2.bin else - set IO_FILE /etc/extras/px4io-v1_default.bin + set IO_FILE /etc/extras/px4io-v1.bin fi if px4io checkcrc ${IO_FILE} @@ -285,6 +356,7 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil + set GPS no if ver hwcmp PX4FMU_V1 then set FMU_MODE serial @@ -313,6 +385,7 @@ then gps start fi fi + unset GPS unset GPS_FAKE # Needs to be this early for in-air-restarts @@ -323,6 +396,14 @@ then # set TTYS1_BUSY no + # + # Check if UAVCAN is enabled, default to it for ESCs + # + if param greater UAVCAN_ENABLE 2 + then + set OUTPUT_MODE uavcan_esc + fi + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output if [ $OUTPUT_MODE != none ] then @@ -396,12 +477,12 @@ then if [ $OUTPUT_MODE == hil ] then - if hil mode_port2_pwm8 + if pwm_out_sim mode_port2_pwm8 then - echo "[i] HIL output started" + echo "[i] PWM SIM output started" else - echo "[i] ERROR: HIL output start failed" - echo "ERROR: HIL output start failed" >> $LOG_FILE + echo "[i] ERROR: PWM SIM output start failed" + echo "ERROR: PWM SIM output start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -456,13 +537,13 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_F "-r 1000 -d /dev/ttyS0" + set MAVLINK_F "-r 1200 -d /dev/ttyS0" # Exit from nsh to free port for mavlink set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - set MAVLINK_F "-r 1000" + set MAVLINK_F "-r 1200" fi fi @@ -478,14 +559,36 @@ then # but this works for now if param compare SYS_COMPANION 921600 then - mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 + mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 80000 -x fi if param compare SYS_COMPANION 57600 then - mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 5000 -x + fi + if param compare SYS_COMPANION 157600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000 + fi + if param compare SYS_COMPANION 257600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x + fi + # Sensors on the PWM interface bank + # clear pins 5 and 6 + if param compare SENS_EN_LL40LS 1 + then + set AUX_MODE pwm4 + fi + if param greater TRIG_MODE 0 + then + # Get FMU driver out of the way + set MIXER_AUX none + set AUX_MODE none + camera_trigger start fi fi + # # UAVCAN # sh /etc/init.d/rc.uavcan @@ -559,7 +662,7 @@ then fi if [ $MIXER == tri_y_yaw- -o $MIXER == tri_y_yaw+ ] then - set MAV_TYPE 2 + set MAV_TYPE 15 fi if [ $MIXER == hexa_x -o $MIXER == hexa_+ ] then @@ -583,6 +686,7 @@ then if [ $MAV_TYPE == none ] then echo "Unknown MAV_TYPE" + param set MAV_TYPE 2 else param set MAV_TYPE $MAV_TYPE fi @@ -620,12 +724,17 @@ then then set MAV_TYPE 21 fi + if [ $MIXER == quad_x_pusher_vtol ] + then + set MAV_TYPE 22 + fi fi # Still no MAV_TYPE found if [ $MAV_TYPE == none ] then echo "Unknown MAV_TYPE" + param set MAV_TYPE 19 else param set MAV_TYPE $MAV_TYPE fi @@ -656,6 +765,8 @@ then then sh /etc/init.d/rc.axialracing_ax10_apps fi + + param set MAV_TYPE 10 fi unset MIXER @@ -685,14 +796,7 @@ then fi unset FEXTRAS - if [ $EXIT_ON_END == yes ] - then - echo "Exit from nsh" - exit - fi - unset EXIT_ON_END - - # Run no SD alarm last + # Run no SD alarm if [ $LOG_FILE == /dev/null ] then echo "[i] No microSD card found" @@ -703,6 +807,33 @@ then # End of autostart fi -# There is no further processing, so we can free some RAM +# There is no further script processing, so we can free some RAM # XXX potentially unset all script variables. unset TUNE_ERR + +# Boot is complete, inform MAVLink app(s) that the system is now fully up and running +mavlink boot_complete + +# Sensors on the PWM interface bank +if param compare SENS_EN_LL40LS 1 +then + if pwm_input start + then + if ll40ls start pwm + then + fi + fi +fi + +if ver hwcmp PX4FMU_V2 +then + # Check for flow sensor - as it is a background task, launch it last + px4flow start & +fi + +if [ $EXIT_ON_END == yes ] +then + echo "Exit from nsh" + exit +fi +unset EXIT_ON_END diff --git a/ROMFS/px4fmu_common/mixers/FX79.main.mix b/ROMFS/px4fmu_common/mixers/FX79.main.mix index b8879af9ee63..3223eff13343 100755 --- a/ROMFS/px4fmu_common/mixers/FX79.main.mix +++ b/ROMFS/px4fmu_common/mixers/FX79.main.mix @@ -27,13 +27,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 7500 7500 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 +S: 0 0 8500 8500 0 -10000 10000 +S: 0 1 9500 9500 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 7500 7500 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 +S: 0 0 8500 8500 0 -10000 10000 +S: 0 1 -9500 -9500 0 -10000 10000 Output 2 -------- @@ -51,19 +51,3 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 - -Inputs to the mixer come from channel group 2 (payload), channels 0 -(bay servo 1), 1 (bay servo 2) and 3 (drop release). ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 0 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 1 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 2 -10000 -10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/README.md b/ROMFS/px4fmu_common/mixers/README.md index b766d05aaca6..0e591ca288ef 100644 --- a/ROMFS/px4fmu_common/mixers/README.md +++ b/ROMFS/px4fmu_common/mixers/README.md @@ -34,7 +34,7 @@ collection of mixers in order to achieve a specific pattern of actuator outputs. The null mixer definition has the form: - Z: + Z: #### Simple Mixer #### @@ -44,8 +44,8 @@ applying an output scaler. A simple mixer definition begins with: - M: - O: <-ve scale> <+ve scale> + M: + O: <-ve scale> <+ve scale> If is zero, the sum is effectively zero and the mixer will output a fixed value that is constrained by and entries describing the control inputs and their scaling, in the form: - S: <-ve scale> <+ve scale> + S: <-ve scale> <+ve scale> The value identifies the control group from which the scaler will read, and the value an offset within that group. These values are specific to @@ -81,16 +81,16 @@ into a set of actuator outputs intended to drive motor speed controllers. The mixer definition is a single line of the form: -R: + R: The supported geometries include: - 4x - quadrotor in X configuration - 4+ - quadrotor in + configuration - 6x - hexcopter in X configuration - 6+ - hexcopter in + configuration - 8x - octocopter in X configuration - 8+ - octocopter in + configuration + * 4x - quadrotor in X configuration + * 4+ - quadrotor in + configuration + * 6x - hexcopter in X configuration + * 6+ - hexcopter in + configuration + * 8x - octocopter in X configuration + * 8+ - octocopter in + configuration Each of the roll, pitch and yaw scale values determine scaling of the roll, pitch and yaw controls relative to the thrust control. Whilst the calculations @@ -102,4 +102,4 @@ thrust input ranges from 0.0 to 1.0. Output for each actuator is in the range -1.0 to 1.0. In the case where an actuator saturates, all actuator values are rescaled so that -the saturating actuator is limited to 1.0. \ No newline at end of file +the saturating actuator is limited to 1.0. diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index fda841640368..8e31007ff197 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -5,18 +5,18 @@ Tilt mechanism servo mixer --------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 1 4 10000 10000 0 -10000 10000 +S: 1 4 0 20000 -10000 -10000 10000 Elevon mixers ------------- M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 -8000 -8000 0 -10000 10000 Landing gear mixer diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 6d72ecc6c74c..0df6c2907113 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -55,11 +55,11 @@ else echo "[param] FAILED loading $PARAM_FILE" fi -if [ -f /etc/extras/px4io-v2_default.bin ] +if [ -f /etc/extras/px4io-v2.bin ] then - set io_file /etc/extras/px4io-v2_default.bin + set io_file /etc/extras/px4io-v2.bin else - set io_file /etc/extras/px4io-v1_default.bin + set io_file /etc/extras/px4io-v1.bin fi if px4io start From 0b3fb33ba52e2e4e9c9864f47faa9fe2b52f9786 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:10:38 +1100 Subject: [PATCH 244/293] systemcmds: update from upstream --- src/systemcmds/bl_update/bl_update.c | 30 +- src/systemcmds/config/config.c | 88 ++-- src/systemcmds/esc_calib/esc_calib.c | 165 ++++--- src/systemcmds/i2c/i2c.c | 16 +- src/systemcmds/mixer/mixer.cpp | 18 +- src/systemcmds/motor_test/motor_test.c | 31 +- src/systemcmds/mtd/24xxxx_mtd.c | 11 +- src/systemcmds/mtd/mtd.c | 152 ++++--- src/systemcmds/nshterm/nshterm.c | 171 ++++---- src/systemcmds/param/param.c | 96 ++-- src/systemcmds/perf/perf.c | 4 +- src/systemcmds/pwm/pwm.c | 36 +- src/systemcmds/reboot/reboot.c | 19 +- src/systemcmds/reflect/reflect.c | 85 ++-- src/systemcmds/tests/module.mk | 20 +- src/systemcmds/tests/test_adc.c | 19 +- src/systemcmds/tests/test_bson.c | 89 ++-- src/systemcmds/tests/test_conv.cpp | 8 +- src/systemcmds/tests/test_dataman.c | 14 +- src/systemcmds/tests/test_eigen.cpp | 462 +++++++++++++------- src/systemcmds/tests/test_file.c | 20 +- src/systemcmds/tests/test_file2.c | 31 +- src/systemcmds/tests/test_float.c | 2 +- src/systemcmds/tests/test_gpio.c | 12 +- src/systemcmds/tests/test_hott_telemetry.c | 53 ++- src/systemcmds/tests/test_hrt.c | 13 +- src/systemcmds/tests/test_int.c | 14 +- src/systemcmds/tests/test_jig_voltages.c | 7 +- src/systemcmds/tests/test_led.c | 20 +- src/systemcmds/tests/test_mathlib.cpp | 39 +- src/systemcmds/tests/test_mixer.cpp | 132 ++++-- src/systemcmds/tests/test_mount.c | 40 +- src/systemcmds/tests/test_param.c | 97 ---- src/systemcmds/tests/test_ppm_loopback.c | 3 +- src/systemcmds/tests/test_rc.c | 17 +- src/systemcmds/tests/test_sensors.c | 28 +- src/systemcmds/tests/test_servo.c | 3 +- src/systemcmds/tests/test_sleep.c | 2 +- src/systemcmds/tests/test_time.c | 7 +- src/systemcmds/tests/test_uart_baudchange.c | 20 +- src/systemcmds/tests/test_uart_console.c | 1 - src/systemcmds/tests/test_uart_loopback.c | 2 +- src/systemcmds/tests/test_uart_send.c | 1 - src/systemcmds/tests/tests_main.c | 5 +- src/systemcmds/top/top.c | 213 +-------- src/systemcmds/ver/ver.c | 21 +- 46 files changed, 1251 insertions(+), 1086 deletions(-) delete mode 100644 src/systemcmds/tests/test_param.c diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c index 082865a866c9..24271c3315ae 100644 --- a/src/systemcmds/bl_update/bl_update.c +++ b/src/systemcmds/bl_update/bl_update.c @@ -61,33 +61,40 @@ static void setopt(void); int bl_update_main(int argc, char *argv[]) { - if (argc != 2) + if (argc != 2) { errx(1, "missing firmware filename or command"); + } - if (!strcmp(argv[1], "setopt")) + if (!strcmp(argv[1], "setopt")) { setopt(); + } int fd = open(argv[1], O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "open %s", argv[1]); + } struct stat s; - if (stat(argv[1], &s) != 0) + if (stat(argv[1], &s) != 0) { err(1, "stat %s", argv[1]); + } /* sanity-check file size */ - if (s.st_size > BL_FILE_SIZE_LIMIT) + if (s.st_size > BL_FILE_SIZE_LIMIT) { errx(1, "%s: file too large (limit: %u, actual: %d)", argv[1], BL_FILE_SIZE_LIMIT, s.st_size); + } uint8_t *buf = malloc(s.st_size); - if (buf == NULL) + if (buf == NULL) { errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size); + } - if (read(fd, buf, s.st_size) != s.st_size) + if (read(fd, buf, s.st_size) != s.st_size) { err(1, "firmware read error"); + } close(fd); @@ -193,24 +200,27 @@ setopt(void) const uint16_t opt_mask = (3 << 2); /* BOR_LEV bitmask */ const uint16_t opt_bits = (0 << 2); /* BOR = 0, setting for 2.7-3.6V operation */ - if ((*optcr & opt_mask) == opt_bits) + if ((*optcr & opt_mask) == opt_bits) { errx(0, "option bits are already set as required"); + } /* unlock the control register */ volatile uint32_t *optkeyr = (volatile uint32_t *)0x40023c08; *optkeyr = 0x08192a3bU; *optkeyr = 0x4c5d6e7fU; - if (*optcr & 1) + if (*optcr & 1) { errx(1, "option control register unlock failed"); + } /* program the new option value */ *optcr = (*optcr & ~opt_mask) | opt_bits | (1 << 1); usleep(1000); - if ((*optcr & opt_mask) == opt_bits) + if ((*optcr & opt_mask) == opt_bits) { errx(0, "option bits set"); + } errx(1, "option bits setting failed; readback 0x%04x", *optcr); diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 210b4caa1f55..362af6535364 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -71,12 +71,15 @@ int config_main(int argc, char *argv[]) { if (argc >= 2) { - if (!strncmp(argv[1], "/dev/gyro",9)) { + if (!strncmp(argv[1], "/dev/gyro", 9)) { do_gyro(argc - 1, argv + 1); - } else if (!strncmp(argv[1], "/dev/accel",10)) { + + } else if (!strncmp(argv[1], "/dev/accel", 10)) { do_accel(argc - 1, argv + 1); - } else if (!strncmp(argv[1], "/dev/mag",8)) { + + } else if (!strncmp(argv[1], "/dev/mag", 8)) { do_mag(argc - 1, argv + 1); + } else { do_device(argc - 1, argv + 1); } @@ -109,16 +112,18 @@ do_device(int argc, char *argv[]) /* disable the device publications */ ret = ioctl(fd, DEVIOCSPUBBLOCK, 1); - if (ret) - errx(ret,"uORB publications could not be blocked"); + if (ret) { + errx(ret, "uORB publications could not be blocked"); + } } else if (argc == 2 && !strcmp(argv[1], "unblock")) { /* enable the device publications */ ret = ioctl(fd, DEVIOCSPUBBLOCK, 0); - if (ret) - errx(ret,"uORB publications could not be unblocked"); + if (ret) { + errx(ret, "uORB publications could not be unblocked"); + } } else { errx(1, "no valid command: %s", argv[1]); @@ -148,24 +153,27 @@ do_gyro(int argc, char *argv[]) /* set the gyro internal sampling rate up to at least i Hz */ ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"sampling rate could not be set"); + if (ret) { + errx(ret, "sampling rate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"pollrate could not be set"); + if (ret) { + errx(ret, "pollrate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i dps */ ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"range could not be set"); + if (ret) { + errx(ret, "range could not be set"); + } } else if (argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, GYROIOCSELFTEST, 0); @@ -181,6 +189,7 @@ do_gyro(int argc, char *argv[]) warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); + } else { warnx("gyro calibration and self test OK"); } @@ -192,12 +201,13 @@ do_gyro(int argc, char *argv[]) int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, GYROIOCGRANGE, 0); - int id = ioctl(fd, DEVIOCGDEVICEID,0); + int id = ioctl(fd, DEVIOCGDEVICEID, 0); int32_t calibration_id = 0; param_get(param_find("CAL_GYRO0_ID"), &(calibration_id)); - warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range); + warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", + id, calibration_id, srate, prate, range); close(fd); } @@ -225,26 +235,29 @@ do_mag(int argc, char *argv[]) /* set the mag internal sampling rate up to at least i Hz */ ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"sampling rate could not be set"); + if (ret) { + errx(ret, "sampling rate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"pollrate could not be set"); + if (ret) { + errx(ret, "pollrate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"range could not be set"); + if (ret) { + errx(ret, "range could not be set"); + } - } else if(argc == 2 && !strcmp(argv[1], "check")) { + } else if (argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret) { @@ -258,6 +271,7 @@ do_mag(int argc, char *argv[]) warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); + } else { warnx("mag calibration and self test OK"); } @@ -269,12 +283,13 @@ do_mag(int argc, char *argv[]) int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, MAGIOCGRANGE, 0); - int id = ioctl(fd, DEVIOCGDEVICEID,0); + int id = ioctl(fd, DEVIOCGDEVICEID, 0); int32_t calibration_id = 0; param_get(param_find("CAL_MAG0_ID"), &(calibration_id)); - warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); + warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", + id, calibration_id, srate, prate, range); close(fd); } @@ -302,26 +317,29 @@ do_accel(int argc, char *argv[]) /* set the accel internal sampling rate up to at least i Hz */ ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"sampling rate could not be set"); + if (ret) { + errx(ret, "sampling rate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"pollrate could not be set"); + if (ret) { + errx(ret, "pollrate could not be set"); + } } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0)); - if (ret) - errx(ret,"range could not be set"); + if (ret) { + errx(ret, "range could not be set"); + } - } else if(argc == 2 && !strcmp(argv[1], "check")) { + } else if (argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret) { @@ -335,23 +353,25 @@ do_accel(int argc, char *argv[]) warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); + } else { warnx("accel calibration and self test OK"); } } else { - errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t"); + errx(1, "no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t"); } int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, ACCELIOCGRANGE, 0); - int id = ioctl(fd, DEVIOCGDEVICEID,0); + int id = ioctl(fd, DEVIOCGDEVICEID, 0); int32_t calibration_id = 0; param_get(param_find("CAL_ACC0_ID"), &(calibration_id)); - warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range); + warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", + id, calibration_id, srate, prate, range); close(fd); } diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index f04ca29ff9cf..86939ff686ac 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -39,7 +39,9 @@ */ #include -#include +#include +#include +#include #include #include @@ -52,15 +54,9 @@ #include #include -#include -#include -#include -#include - #include #include "systemlib/systemlib.h" -#include "systemlib/err.h" #include "drivers/drv_pwm_output.h" #include @@ -75,10 +71,11 @@ __EXPORT int esc_calib_main(int argc, char *argv[]); static void usage(const char *reason) { - if (reason != NULL) - warnx("%s", reason); + if (reason != NULL) { + PX4_ERR("%s", reason); + } - errx(1, + PX4_ERR( "usage:\n" "esc_calib\n" " [-d PWM output device (defaults to " PWM_OUTPUT0_DEVICE_PATH ")\n" @@ -93,7 +90,7 @@ usage(const char *reason) int esc_calib_main(int argc, char *argv[]) { - char *dev = PWM_OUTPUT0_DEVICE_PATH; + const char *dev = PWM_OUTPUT0_DEVICE_PATH; char *ep; int ch; int ret; @@ -114,66 +111,90 @@ esc_calib_main(int argc, char *argv[]) if (argc < 2) { usage("no channels provided"); + return 1; } int arg_consumed = 0; - while ((ch = getopt(argc, argv, "d:c:m:al:h:")) != EOF) { + int myoptind = 1; + const char *myoptarg = NULL; + + while ((ch = px4_getopt(argc, argv, "d:c:m:al:h:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'd': - dev = optarg; + dev = myoptarg; arg_consumed += 2; break; case 'c': /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ - channels = strtoul(optarg, &ep, 0); + channels = strtoul(myoptarg, &ep, 0); while ((single_ch = channels % 10)) { - set_mask |= 1<<(single_ch-1); + set_mask |= 1 << (single_ch - 1); channels /= 10; } + break; case 'm': /* Read in mask directly */ - set_mask = strtoul(optarg, &ep, 0); - if (*ep != '\0') + set_mask = strtoul(myoptarg, &ep, 0); + + if (*ep != '\0') { usage("bad set_mask value"); + return 1; + } + break; case 'a': + /* Choose all channels */ - for (unsigned i = 0; i PWM_HIGHEST_MIN) + pwm_low = strtoul(myoptarg, &ep, 0); + + if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN) { usage("low PWM invalid"); + return 1; + } + break; + case 'h': /* Read in custom high value */ - pwm_high = strtoul(optarg, &ep, 0); - if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) + pwm_high = strtoul(myoptarg, &ep, 0); + + if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) { usage("high PWM invalid"); + return 1; + } + break; + default: usage(NULL); + return 1; } } if (set_mask == 0) { usage("no channels chosen"); + return 1; } if (pwm_low > pwm_high) { usage("low pwm is higher than high pwm"); + return 1; } /* make sure no other source is publishing control values now */ @@ -191,9 +212,10 @@ esc_calib_main(int argc, char *argv[]) orb_check(act_sub, &orb_updated); if (orb_updated) { - errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" + PX4_ERR("ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" "\tmc_att_control stop\n" "\tfw_att_control stop\n"); + return 1; } printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" @@ -211,24 +233,25 @@ esc_calib_main(int argc, char *argv[]) ret = poll(&fds, 1, 0); if (ret > 0) { - - read(0, &c, 1); + if (read(0, &c, 1) <= 0) { + printf("ESC calibration read error\n"); + return 0; + } if (c == 'y' || c == 'Y') { - break; } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); - exit(0); + return 0; } else if (c == 'n' || c == 'N') { printf("ESC calibration aborted\n"); - exit(0); + return 0; } else { printf("Unknown input, ESC calibration aborted\n"); - exit(0); + return 0; } } @@ -239,24 +262,36 @@ esc_calib_main(int argc, char *argv[]) /* open for ioctl only */ int fd = open(dev, 0); - if (fd < 0) - err(1, "can't open %s", dev); + if (fd < 0) { + PX4_ERR("can't open %s", dev); + return 1; + } /* get number of channels available on the device */ ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels); - if (ret != OK) - err(1, "PWM_SERVO_GET_COUNT"); + + if (ret != OK) { + PX4_ERR("PWM_SERVO_GET_COUNT"); + return 1; + } /* tell IO/FMU that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - if (ret != OK) - err(1, "PWM_SERVO_SET_ARM_OK"); + + if (ret != OK) { + PX4_ERR("PWM_SERVO_SET_ARM_OK"); + return 1; + } + /* tell IO/FMU that the system is armed (it will output values if safety is off) */ ret = ioctl(fd, PWM_SERVO_ARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_ARM"); - warnx("Outputs armed"); + if (ret != OK) { + PX4_ERR("PWM_SERVO_ARM"); + return 1; + } + + printf("Outputs armed"); /* wait for user confirmation */ @@ -270,27 +305,30 @@ esc_calib_main(int argc, char *argv[]) /* set max PWM */ for (unsigned i = 0; i < max_channels; i++) { - if (set_mask & 1< 0) { - - read(0, &c, 1); + if (read(0, &c, 1) <= 0) { + printf("ESC calibration read error\n"); + goto done; + } if (c == 13) { - break; } else if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("ESC calibration exited"); - exit(0); + printf("ESC calibration exited"); + goto done; } } @@ -307,27 +345,30 @@ esc_calib_main(int argc, char *argv[]) /* set disarmed PWM */ for (unsigned i = 0; i < max_channels; i++) { - if (set_mask & 1< 0) { - - read(0, &c, 1); + if (read(0, &c, 1) <= 0) { + printf("ESC calibration read error\n"); + goto done; + } if (c == 13) { - break; } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); - exit(0); + goto done; } } @@ -337,12 +378,20 @@ esc_calib_main(int argc, char *argv[]) /* disarm */ ret = ioctl(fd, PWM_SERVO_DISARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_DISARM"); - warnx("Outputs disarmed"); + if (ret != OK) { + PX4_ERR("PWM_SERVO_DISARM"); + goto cleanup; + } + + printf("Outputs disarmed"); printf("ESC calibration finished\n"); - exit(0); +done: + close(fd); + return 0; +cleanup: + close(fd); + return 1; } diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c index 2991456cf1d4..1fa60a7e47fd 100644 --- a/src/systemcmds/i2c/i2c.c +++ b/src/systemcmds/i2c/i2c.c @@ -75,21 +75,25 @@ int i2c_main(int argc, char *argv[]) /* find the right I2C */ i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); - if (i2c == NULL) + if (i2c == NULL) { errx(1, "failed to locate I2C bus"); + } usleep(100000); uint8_t buf[] = { 0, 4}; int ret = transfer(PX4_I2C_OBDEV_PX4IO, buf, sizeof(buf), NULL, 0); - if (ret) + if (ret) { errx(1, "send failed - %d", ret); + } uint32_t val; ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val)); - if (ret) + + if (ret) { errx(1, "recive failed - %d", ret); + } errx(0, "got 0x%08x", val); } @@ -121,8 +125,9 @@ transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsig msgs++; } - if (msgs == 0) + if (msgs == 0) { return -1; + } /* * I2C architecture means there is an unavoidable race here @@ -133,8 +138,9 @@ transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsig ret = I2C_TRANSFER(i2c, &msgv[0], msgs); // reset the I2C bus to unwedge on error - if (ret != OK) + if (ret != OK) { up_i2creset(i2c); + } return ret; } diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 41add4973aec..c83eabc9a7aa 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -76,30 +76,37 @@ mixer_main(int argc, char *argv[]) } int ret = load(argv[2], argv[3]); - if(ret !=0) { + + if (ret != 0) { warnx("failed to load mixer"); return 1; } + } else { usage("Unknown command"); return 1; } + return 0; } static void usage(const char *reason) { - if (reason) - fprintf(stderr, "%s\n", reason); + if (reason && *reason) { + PX4_INFO("%s", reason); + } - fprintf(stderr, "usage:\n"); - fprintf(stderr, " mixer load \n"); + PX4_INFO("usage:"); + PX4_INFO(" mixer load "); } static int load(const char *devname, const char *fname) { + // sleep a while to ensure device has been set up + usleep(20000); + int dev; char buf[2048]; @@ -127,5 +134,6 @@ load(const char *devname, const char *fname) warnx("error loading mixers from %s", fname); return 1; } + return 0; } diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 87ea1b13eabb..3581c2004bd9 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -59,7 +59,7 @@ __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 = NULL; +static orb_advert_t _test_motor_pub = nullptr; void motor_test(unsigned channel, float value) { @@ -69,13 +69,14 @@ void motor_test(unsigned channel, float value) _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub != NULL) { - /* 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); - } + if (_test_motor_pub != nullptr) { + /* 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) @@ -85,10 +86,10 @@ static void usage(const char *reason) } errx(1, - "usage:\n" - "motor_test\n" - " -m Motor to test (0..7)\n" - " -p Power (0..100)\n"); + "usage:\n" + "motor_test\n" + " -m Motor to test (0..7)\n" + " -p Power (0..100)\n"); } int motor_test_main(int argc, char *argv[]) @@ -114,11 +115,13 @@ int motor_test_main(int argc, char *argv[]) /* Read in power value */ lval = strtoul(optarg, NULL, 0); - if (lval > 100) + if (lval > 100) { usage("value invalid"); + } - value = ((float)lval)/100.f; + value = ((float)lval) / 100.f; break; + default: usage(NULL); } diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index 2aba9dbb69fa..9fc27868310f 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -239,11 +239,13 @@ void at24c_test(void) for (count = 0; count < 10000; count++) { ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); + if (result == ERROR) { if (errors++ > 2) { vdbg("too many errors\n"); return; } + } else if (result != 1) { vdbg("unexpected %u\n", result); } @@ -311,8 +313,9 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); perf_end(priv->perf_transfers); - if (ret >= 0) + if (ret >= 0) { break; + } fvdbg("read stall"); usleep(1000); @@ -396,8 +399,9 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); perf_end(priv->perf_transfers); - if (ret >= 0) + if (ret >= 0) { break; + } fvdbg("write stall"); usleep(1000); @@ -503,7 +507,8 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) * ************************************************************************************/ -FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { +FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) +{ FAR struct at24c_dev_s *priv; fvdbg("dev: %p\n", dev); diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index c1dbcad7099d..b38504c623df 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -92,8 +92,8 @@ static void mtd_erase(char *partition_names[], unsigned n_partitions); static void mtd_readtest(char *partition_names[], unsigned n_partitions); static void mtd_rwtest(char *partition_names[], unsigned n_partitions); static void mtd_print_info(void); -static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, - unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); +static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); static bool attached = false; static bool started = false; @@ -107,9 +107,10 @@ static const int n_partitions_default = sizeof(partition_names_default) / sizeof static void mtd_status(void) { - if (!attached) + if (!attached) { errx(1, "MTD driver not started"); - + } + mtd_print_info(); exit(0); } @@ -122,40 +123,46 @@ int mtd_main(int argc, char *argv[]) /* start mapping according to user request */ if (argc >= 3) { mtd_start(argv + 2, argc - 2); + } else { mtd_start(partition_names_default, n_partitions_default); } } - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { mtd_test(); + } if (!strcmp(argv[1], "readtest")) { if (argc >= 3) { mtd_readtest(argv + 2, argc - 2); + } else { mtd_readtest(partition_names_default, n_partitions_default); } - } + } if (!strcmp(argv[1], "rwtest")) { if (argc >= 3) { mtd_rwtest(argv + 2, argc - 2); + } else { mtd_rwtest(partition_names_default, n_partitions_default); } - } + } - if (!strcmp(argv[1], "status")) + if (!strcmp(argv[1], "status")) { mtd_status(); + } if (!strcmp(argv[1], "erase")) { if (argc >= 3) { mtd_erase(argv + 2, argc - 2); + } else { mtd_erase(partition_names_default, n_partitions_default); } - } + } } errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'"); @@ -163,26 +170,24 @@ int mtd_main(int argc, char *argv[]) struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, - off_t firstblock, off_t nblocks); + off_t firstblock, off_t nblocks); #ifdef CONFIG_MTD_RAMTRON static void ramtron_attach(void) { - /* find the right spi */ -#ifdef CONFIG_ARCH_BOARD_AEROCORE - struct spi_dev_s *spi = up_spiinitialize(4); -#else - struct spi_dev_s *spi = up_spiinitialize(2); -#endif + /* initialize the right spi */ + struct spi_dev_s *spi = up_spiinitialize(PX4_SPI_BUS_RAMTRON); + /* this resets the spi bus, set correct bus speed again */ SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); SPI_SETBITS(spi, 8); SPI_SETMODE(spi, SPIDEV_MODE3); SPI_SELECT(spi, SPIDEV_FLASH, false); - if (spi == NULL) + if (spi == NULL) { errx(1, "failed to locate spi bus"); + } /* start the RAMTRON driver, attempt 5 times */ for (int i = 0; i < 5; i++) { @@ -199,10 +204,12 @@ ramtron_attach(void) } /* if last attempt is still unsuccessful, abort */ - if (mtd_dev == NULL) + if (mtd_dev == NULL) { errx(1, "failed to initialize mtd driver"); + } + + int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10 * 1000 * 1000); - int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); if (ret != OK) { // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried // that but setting the bug speed does fail all the time. Which was then exiting and the board would @@ -222,24 +229,28 @@ at24xxx_attach(void) /* this resets the I2C bus, set correct bus speed again */ I2C_SETFREQUENCY(i2c, 400000); - if (i2c == NULL) + if (i2c == NULL) { errx(1, "failed to locate I2C bus"); + } /* start the MTD driver, attempt 5 times */ for (int i = 0; i < 5; i++) { mtd_dev = at24c_initialize(i2c); + if (mtd_dev) { /* abort on first valid result */ if (i > 0) { - warnx("warning: EEPROM needed %d attempts to attach", i+1); + warnx("warning: EEPROM needed %d attempts to attach", i + 1); } + break; } } /* if last attempt is still unsuccessful, abort */ - if (mtd_dev == NULL) + if (mtd_dev == NULL) { errx(1, "failed to initialize EEPROM driver"); + } attached = true; } @@ -250,15 +261,16 @@ mtd_start(char *partition_names[], unsigned n_partitions) { int ret; - if (started) + if (started) { errx(1, "mtd already mounted"); + } if (!attached) { - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - at24xxx_attach(); - #else +#ifdef CONFIG_MTD_RAMTRON ramtron_attach(); - #endif +#else + at24xxx_attach(); +#endif } if (!mtd_dev) { @@ -270,8 +282,10 @@ mtd_start(char *partition_names[], unsigned n_partitions) unsigned blkpererase, nblocks, partsize; ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions); - if (ret) + + if (ret) { exit(3); + } /* Now create MTD FLASH partitions */ @@ -320,10 +334,10 @@ mtd_start(char *partition_names[], unsigned n_partitions) exit(0); } -int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, - unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions) +int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions) { - /* Get the geometry of the FLASH device */ + /* Get the geometry of the FLASH device */ FAR struct mtd_geometry_s geo; @@ -358,24 +372,31 @@ static ssize_t mtd_get_partition_size(void) unsigned long blocksize, erasesize, neraseblocks; unsigned blkpererase, nblocks, partsize = 0; - int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, + n_partitions_current); + if (ret != OK) { errx(1, "Failed to get geometry"); } + return partsize; } void mtd_print_info(void) { - if (!attached) + if (!attached) { exit(1); + } unsigned long blocksize, erasesize, neraseblocks; unsigned blkpererase, nblocks, partsize; - int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); - if (ret) + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, + n_partitions_current); + + if (ret) { exit(3); + } warnx("Flash Geometry:"); @@ -400,19 +421,24 @@ mtd_erase(char *partition_names[], unsigned n_partitions) { uint8_t v[64]; memset(v, 0xFF, sizeof(v)); + for (uint8_t i = 0; i < n_partitions; i++) { uint32_t count = 0; printf("Erasing %s\n", partition_names[i]); int fd = open(partition_names[i], O_WRONLY); + if (fd == -1) { errx(1, "Failed to open partition"); } + while (write(fd, v, sizeof(v)) == sizeof(v)) { count += sizeof(v); } + printf("Erased %lu bytes\n", (unsigned long)count); close(fd); } + exit(0); } @@ -427,21 +453,27 @@ mtd_readtest(char *partition_names[], unsigned n_partitions) ssize_t expected_size = mtd_get_partition_size(); uint8_t v[128]; + for (uint8_t i = 0; i < n_partitions; i++) { ssize_t count = 0; printf("reading %s expecting %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDONLY); + if (fd == -1) { errx(1, "Failed to open partition"); } + while (read(fd, v, sizeof(v)) == sizeof(v)) { count += sizeof(v); } + if (count != expected_size) { - errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + errx(1, "Failed to read partition - got %u/%u bytes", count, expected_size); } + close(fd); } + printf("readtest OK\n"); exit(0); } @@ -458,38 +490,50 @@ mtd_rwtest(char *partition_names[], unsigned n_partitions) ssize_t expected_size = mtd_get_partition_size(); uint8_t v[128], v2[128]; + for (uint8_t i = 0; i < n_partitions; i++) { ssize_t count = 0; - off_t offset = 0; + off_t offset = 0; printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDWR); + if (fd == -1) { errx(1, "Failed to open partition"); } + while (read(fd, v, sizeof(v)) == sizeof(v)) { count += sizeof(v); - if (lseek(fd, offset, SEEK_SET) != offset) { - errx(1, "seek failed"); - } - if (write(fd, v, sizeof(v)) != sizeof(v)) { - errx(1, "write failed"); - } - if (lseek(fd, offset, SEEK_SET) != offset) { - errx(1, "seek failed"); - } - if (read(fd, v2, sizeof(v2)) != sizeof(v2)) { - errx(1, "read failed"); - } - if (memcmp(v, v2, sizeof(v2)) != 0) { - errx(1, "memcmp failed"); - } - offset += sizeof(v); + + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + + if (write(fd, v, sizeof(v)) != sizeof(v)) { + errx(1, "write failed"); + } + + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + + if (read(fd, v2, sizeof(v2)) != sizeof(v2)) { + errx(1, "read failed"); + } + + if (memcmp(v, v2, sizeof(v2)) != 0) { + errx(1, "memcmp failed"); + } + + offset += sizeof(v); } + if (count != expected_size) { - errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + errx(1, "Failed to read partition - got %u/%u bytes", count, expected_size); } + close(fd); } + printf("rwtest OK\n"); exit(0); } diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 5690815322b6..56d4f9f3af23 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -59,87 +59,92 @@ __EXPORT int nshterm_main(int argc, char *argv[]); int nshterm_main(int argc, char *argv[]) { - if (argc < 2) { - printf("Usage: nshterm \n"); - exit(1); - } - unsigned retries = 0; - int fd = -1; - 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) { - - /* abort if an arming topic is published and system is armed */ - bool updated = false; - orb_check(armed_fd, &updated); - if (updated) { - /* the system is now providing arming status feedback. - * instead of timing out, we resort to abort bringing - * up the terminal. - */ - orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); - - if (armed.armed) { - /* this is not an error, but we are done */ - exit(0); - } - } - - /* the retries are to cope with the behaviour of /dev/ttyACM0 */ - /* which may not be ready immediately. */ - fd = open(argv[1], O_RDWR); - if (fd != -1) { - close(armed_fd); - break; - } - usleep(100000); - retries++; - } - if (fd == -1) { - perror(argv[1]); - exit(1); - } - - /* set up the serial port with output processing */ - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; - - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { - warnx("ERR get config %s: %d\n", argv[1], termios_state); - close(fd); - return -1; - } - - /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST); - - if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERR set config %s\n", argv[1]); - close(fd); - return -1; - } - - /* setup standard file descriptors */ - close(0); - close(1); - close(2); - dup2(fd, 0); - dup2(fd, 1); - dup2(fd, 2); - - nsh_consolemain(0, NULL); - - close(fd); - - return OK; + if (argc < 2) { + printf("Usage: nshterm \n"); + exit(1); + } + + unsigned retries = 0; + int fd = -1; + int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); + struct actuator_armed_s armed; + + /* back off 1800 ms to avoid running into the USB setup timing */ + while (hrt_absolute_time() < 1800U * 1000U) { + usleep(50000); + } + + /* try to bring up the console - stop doing so if the system gets armed */ + while (true) { + + /* abort if an arming topic is published and system is armed */ + bool updated = false; + orb_check(armed_fd, &updated); + + if (updated) { + /* the system is now providing arming status feedback. + * instead of timing out, we resort to abort bringing + * up the terminal. + */ + orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); + + if (armed.armed) { + /* this is not an error, but we are done */ + exit(0); + } + } + + /* the retries are to cope with the behaviour of /dev/ttyACM0 */ + /* which may not be ready immediately. */ + fd = open(argv[1], O_RDWR); + + if (fd != -1) { + close(armed_fd); + break; + } + + usleep(100000); + retries++; + } + + if (fd == -1) { + perror(argv[1]); + exit(1); + } + + /* set up the serial port with output processing */ + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { + warnx("ERR get config %s: %d\n", argv[1], termios_state); + close(fd); + return -1; + } + + /* Set ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag |= (ONLCR | OPOST); + + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR set config %s\n", argv[1]); + close(fd); + return -1; + } + + /* setup standard file descriptors */ + close(0); + close(1); + close(2); + dup2(fd, 0); + dup2(fd, 1); + dup2(fd, 2); + + nsh_consolemain(0, NULL); + + close(fd); + + return OK; } diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 2e0f1c104786..88f5197d27b1 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -59,14 +59,19 @@ __EXPORT int param_main(int argc, char *argv[]); +enum COMPARE_OPERATOR { + COMPARE_OPERATOR_EQUAL = 0, + COMPARE_OPERATOR_GREATER = 1, +}; + static int do_save(const char *param_file_name); static int do_load(const char *param_file_name); static int do_import(const char *param_file_name); -static void do_show(const char *search_string); -static void do_show_index(const char *index, bool used_index); +static int do_show(const char *search_string); +static int do_show_index(const char *index, bool used_index); static void do_show_print(void *arg, param_t param); static int do_set(const char *name, const char *val, bool fail_on_not_found); -static int do_compare(const char *name, char *vals[], unsigned comparisons); +static int do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OPERATOR cmd_op); static int do_reset(const char *excludes[], int num_excludes); static int do_reset_nostart(const char *excludes[], int num_excludes); @@ -121,12 +126,10 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "show")) { if (argc >= 3) { - do_show(argv[2]); - return 0; + return do_show(argv[2]); } else { - do_show(NULL); - return 0; + return do_show(NULL); } } @@ -149,7 +152,7 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "compare")) { if (argc >= 4) { - return do_compare(argv[2], &argv[3], argc - 3); + return do_compare(argv[2], &argv[3], argc - 3, COMPARE_OPERATOR_EQUAL); } else { warnx("not enough arguments.\nTry 'param compare PARAM_NAME 3'"); @@ -157,6 +160,16 @@ param_main(int argc, char *argv[]) } } + if (!strcmp(argv[1], "greater")) { + if (argc >= 4) { + return do_compare(argv[2], &argv[3], argc - 3, COMPARE_OPERATOR_GREATER); + + } else { + warnx("not enough arguments.\nTry 'param greater PARAM_NAME 3'"); + return 1; + } + } + if (!strcmp(argv[1], "reset")) { if (argc >= 3) { return do_reset((const char **) &argv[2], argc - 2); @@ -177,7 +190,8 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "index_used")) { if (argc >= 3) { - do_show_index(argv[2], true); + return do_show_index(argv[2], true); + } else { warnx("no index provided"); return 1; @@ -186,7 +200,8 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "index")) { if (argc >= 3) { - do_show_index(argv[2], false); + return do_show_index(argv[2], false); + } else { warnx("no index provided"); return 1; @@ -194,7 +209,7 @@ param_main(int argc, char *argv[]) } } - warnx("expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); + warnx("expected a command, try 'load', 'import', 'show', 'set', 'compare',\n'index', 'index_used', 'select' or 'save'"); return 1; } @@ -202,7 +217,7 @@ static int do_save(const char *param_file_name) { /* create the file */ - int fd = px4_open(param_file_name, O_WRONLY | O_CREAT, 0x777); + int fd = open(param_file_name, O_WRONLY | O_CREAT, PX4_O_MODE_666); if (fd < 0) { warn("opening '%s' failed", param_file_name); @@ -210,10 +225,12 @@ do_save(const char *param_file_name) } int result = param_export(fd, false); - px4_close(fd); + close(fd); if (result < 0) { +#ifndef __PX4_QURT (void)unlink(param_file_name); +#endif warnx("error exporting to '%s'", param_file_name); return 1; } @@ -224,15 +241,15 @@ do_save(const char *param_file_name) static int do_load(const char *param_file_name) { - int fd = px4_open(param_file_name, O_RDONLY); + int fd = open(param_file_name, O_RDONLY); if (fd < 0) { - warn("open '%s'", param_file_name); + warn("open failed '%s'", param_file_name); return 1; } int result = param_load(fd); - px4_close(fd); + close(fd); if (result < 0) { warnx("error importing from '%s'", param_file_name); @@ -245,7 +262,7 @@ do_load(const char *param_file_name) static int do_import(const char *param_file_name) { - int fd = px4_open(param_file_name, O_RDONLY); + int fd = open(param_file_name, O_RDONLY); if (fd < 0) { warn("open '%s'", param_file_name); @@ -253,7 +270,7 @@ do_import(const char *param_file_name) } int result = param_import(fd); - px4_close(fd); + close(fd); if (result < 0) { warnx("error importing from '%s'", param_file_name); @@ -263,15 +280,17 @@ do_import(const char *param_file_name) return 0; } -static void +static int do_show(const char *search_string) { printf("Symbols: x = used, + = saved, * = unsaved\n"); param_foreach(do_show_print, (char *)search_string, false, false); printf("\n %u parameters total, %u used.\n", param_count(), param_count_used()); + + return 0; } -static void +static int do_show_index(const char *index, bool used_index) { char *end; @@ -282,12 +301,14 @@ do_show_index(const char *index, bool used_index) if (used_index) { param = param_for_used_index(i); + } else { param = param_for_index(i); } if (param == PARAM_INVALID) { - return; + warnx("param not found for index %u", i); + return 1; } printf("index %d: %c %c %s [%d,%d] : ", i, (param_used(param) ? 'x' : ' '), @@ -308,11 +329,12 @@ do_show_index(const char *index, bool used_index) } break; + default: printf("\n", 0 + param_type(param)); } - exit(0); + return 0; } static void @@ -409,10 +431,6 @@ do_set(const char *name, const char *val, bool fail_on_not_found) return (fail_on_not_found) ? 1 : 0; } - printf("%c %s: ", - param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), - param_name(param)); - /* * Set parameter if type is known and conversion from string to value turns out fine */ @@ -425,10 +443,10 @@ do_set(const char *name, const char *val, bool fail_on_not_found) char *end; int32_t newval = strtol(val, &end, 10); - if (i == newval) { - printf("unchanged\n"); - - } else { + if (i != newval) { + printf("%c %s: ", + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); printf("curr: %ld", (long)i); param_set(param, &newval); printf(" -> new: %ld\n", (long)newval); @@ -446,11 +464,11 @@ do_set(const char *name, const char *val, bool fail_on_not_found) #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wfloat-equal" - if (f == newval) { + if (f != newval) { #pragma GCC diagnostic pop - printf("unchanged\n"); - - } else { + printf("%c %s: ", + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); printf("curr: %4.4f", (double)f); param_set(param, &newval); printf(" -> new: %4.4f\n", (double)newval); @@ -469,7 +487,7 @@ do_set(const char *name, const char *val, bool fail_on_not_found) } static int -do_compare(const char *name, char *vals[], unsigned comparisons) +do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OPERATOR cmp_op) { int32_t i; float f; @@ -499,7 +517,8 @@ do_compare(const char *name, char *vals[], unsigned comparisons) int j = strtol(vals[k], &end, 10); - if (i == j) { + if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (i == j)) || + ((cmp_op == COMPARE_OPERATOR_GREATER) && (i > j))) { printf(" %ld: ", (long)i); ret = 0; } @@ -518,7 +537,8 @@ do_compare(const char *name, char *vals[], unsigned comparisons) float g = strtod(vals[k], &end); - if (fabsf(f - g) < 1e-7f) { + if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (fabsf(f - g) < 1e-7f)) || + ((cmp_op == COMPARE_OPERATOR_GREATER) && (f > g))) { printf(" %4.4f: ", (double)f); ret = 0; } @@ -555,6 +575,7 @@ do_reset(const char *excludes[], int num_excludes) warnx("Param export failed."); return 1; } + return 0; } @@ -582,5 +603,6 @@ do_reset_nostart(const char *excludes[], int num_excludes) return 1; } + return 0; } diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c index 49551aec93c8..fb4e5030fc27 100644 --- a/src/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -70,7 +70,7 @@ int perf_main(int argc, char *argv[]) return 0; } else if (strcmp(argv[1], "latency") == 0) { - perf_print_latency(1 /* stdout */); + perf_print_latency(0 /* stdout */); fflush(stdout); return 0; } @@ -79,7 +79,7 @@ int perf_main(int argc, char *argv[]) return -1; } - perf_print_all(1 /* stdout */); + perf_print_all(0 /* stdout */); fflush(stdout); return 0; } diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index d6b03eb8809a..a11995296319 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -59,6 +59,7 @@ #include "systemlib/systemlib.h" #include "systemlib/err.h" +#include "systemlib/param/param.h" #include "drivers/drv_pwm_output.h" static void usage(const char *reason); @@ -186,11 +187,38 @@ pwm_main(int argc, char *argv[]) break; - case 'p': - pwm_value = strtoul(optarg, &ep, 0); + case 'p': { + /* check if this is a param name */ + if (strncmp("p:", optarg, 2) == 0) { - if (*ep != '\0') { - usage("BAD PWM VAL"); + char buf[32]; + strncpy(buf, optarg + 2, 16); + /* user wants to use a param name */ + param_t parm = param_find(buf); + + if (parm != PARAM_INVALID) { + int32_t pwm_parm; + int gret = param_get(parm, &pwm_parm); + + if (gret == 0) { + pwm_value = pwm_parm; + + } else { + usage("PARAM LOAD FAIL"); + } + + } else { + usage("PARAM NAME NOT FOUND"); + } + + } else { + + pwm_value = strtoul(optarg, &ep, 0); + } + + if (*ep != '\0') { + usage("BAD PWM VAL"); + } } break; diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index de4d77d5fd0c..8309d74ea44c 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -38,12 +38,10 @@ */ #include -#include -#include -#include - +#include +#include +#include #include -#include __EXPORT int reboot_main(int argc, char *argv[]); @@ -52,20 +50,21 @@ int reboot_main(int argc, char *argv[]) int ch; bool to_bootloader = false; - while ((ch = getopt(argc, argv, "b")) != -1) { + int myoptind = 1; + const char *myoptarg = NULL; + + while ((ch = px4_getopt(argc, argv, "b", &myoptind, &myoptarg)) != -1) { switch (ch) { case 'b': to_bootloader = true; break; default: - errx(1, "usage: reboot [-b]\n" - " -b reboot into the bootloader"); + PX4_ERR("usage: reboot [-b]\n" + " -b reboot into the bootloader"); } } px4_systemreset(to_bootloader); } - - diff --git a/src/systemcmds/reflect/reflect.c b/src/systemcmds/reflect/reflect.c index fe5f0f0359d4..683ff7c6eae2 100644 --- a/src/systemcmds/reflect/reflect.c +++ b/src/systemcmds/reflect/reflect.c @@ -54,7 +54,7 @@ __EXPORT int reflect_main(int argc, char *argv[]); #define MAX_BLOCKS 1000 static uint32_t nblocks; struct block { - uint32_t v[256]; + uint32_t v[256]; }; static struct block *blocks[MAX_BLOCKS]; @@ -62,50 +62,59 @@ static struct block *blocks[MAX_BLOCKS]; 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); + while (nblocks < MAX_BLOCKS) { + blocks[nblocks] = calloc(1, sizeof(struct block)); + + if (blocks[nblocks] == NULL) { + break; + } + + for (uint32_t i = 0; i < sizeof(blocks[nblocks]->v) / 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)); - } - } + for (uint32_t n = 0; n < nblocks; n++) { + for (uint32_t i = 0; i < sizeof(blocks[nblocks]->v) / 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; + 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; } diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 74114719fad3..7e3144252599 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -3,8 +3,8 @@ # MODULE_COMMAND = tests -MODULE_STACKSIZE = 12000 -MAXOPTIMIZATION = -Os +MODULE_STACKSIZE = 60000 +MAXOPTIMIZATION = -O0 SRCS = test_adc.c \ test_bson.c \ @@ -18,7 +18,6 @@ SRCS = test_adc.c \ test_sensors.c \ test_servo.c \ test_sleep.c \ - test_time.c \ test_uart_baudchange.c \ test_uart_console.c \ test_uart_loopback.c \ @@ -35,5 +34,18 @@ SRCS = test_adc.c \ test_mount.c \ test_eigen.cpp -EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion -Wno-error=logical-op +ifeq ($(PX4_TARGET_OS), nuttx) +SRCS += test_time.c + +EXTRACXXFLAGS = -Wframe-larger-than=6000 +else +EXTRACXXFLAGS = +endif + +EXTRACXXFLAGS += -Wno-float-equal + +# Flag is only valid for GCC, not clang +ifneq ($(USE_GCC), 0) +EXTRACXXFLAGS += -Wno-double-promotion -Wno-error=logical-op +endif diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index 7cb807d4c72c..ef7f217fdfcb 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -37,7 +37,9 @@ */ #include -#include +#include +#include +#include #include @@ -46,22 +48,17 @@ #include #include #include -#include - -#include #include "tests.h" -#include #include -#include int test_adc(int argc, char *argv[]) { - int fd = open(ADC0_DEVICE_PATH, O_RDONLY); + int fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY); if (fd < 0) { - warnx("ERROR: can't open ADC device"); + PX4_ERR("ERROR: can't open ADC device"); return 1; } @@ -69,7 +66,7 @@ int test_adc(int argc, char *argv[]) /* make space for a maximum of twelve channels */ struct adc_msg_s data[12]; /* read all channels available */ - ssize_t count = read(fd, data, sizeof(data)); + ssize_t count = px4_read(fd, data, sizeof(data)); if (count < 0) { goto errout_with_dev; @@ -85,11 +82,11 @@ int test_adc(int argc, char *argv[]) usleep(150000); } - warnx("\t ADC test successful.\n"); + printf("\t ADC test successful.\n"); errout_with_dev: - if (fd != 0) { close(fd); } + if (fd != 0) { px4_close(fd); } return OK; } diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c index 02384ebfebd4..945b00aa51a6 100644 --- a/src/systemcmds/tests/test_bson.c +++ b/src/systemcmds/tests/test_bson.c @@ -37,9 +37,14 @@ * Tests for the bson en/decoder */ +#define __STDC_FORMAT_MACROS +#include + +#include #include #include #include +#include #include #include @@ -59,27 +64,33 @@ static int encode(bson_encoder_t encoder) { if (bson_encoder_append_bool(encoder, "bool1", sample_bool) != 0) { - warnx("FAIL: encoder: append bool failed"); + PX4_ERR("FAIL: encoder: append bool failed"); + return 1; } if (bson_encoder_append_int(encoder, "int1", sample_small_int) != 0) { - warnx("FAIL: encoder: append int failed"); + PX4_ERR("FAIL: encoder: append int failed"); + return 1; } if (bson_encoder_append_int(encoder, "int2", sample_big_int) != 0) { - warnx("FAIL: encoder: append int failed"); + PX4_ERR("FAIL: encoder: append int failed"); + return 1; } if (bson_encoder_append_double(encoder, "double1", sample_double) != 0) { - warnx("FAIL: encoder: append double failed"); + PX4_ERR("FAIL: encoder: append double failed"); + return 1; } if (bson_encoder_append_string(encoder, "string1", sample_string) != 0) { - warnx("FAIL: encoder: append string failed"); + PX4_ERR("FAIL: encoder: append string failed"); + return 1; } if (bson_encoder_append_binary(encoder, "data1", BSON_BIN_BINARY, sizeof(sample_data), sample_data) != 0) { - warnx("FAIL: encoder: append data failed"); + PX4_ERR("FAIL: encoder: append data failed"); + return 1; } bson_encoder_fini(encoder); @@ -94,29 +105,29 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) if (!strcmp(node->name, "bool1")) { if (node->type != BSON_BOOL) { - warnx("FAIL: decoder: bool1 type %d, expected %d", node->type, BSON_BOOL); + PX4_ERR("FAIL: decoder: bool1 type %d, expected %d", node->type, BSON_BOOL); return 1; } if (node->b != sample_bool) { - warnx("FAIL: decoder: bool1 value %s, expected %s", - (node->b ? "true" : "false"), - (sample_bool ? "true" : "false")); + PX4_ERR("FAIL: decoder: bool1 value %s, expected %s", + (node->b ? "true" : "false"), + (sample_bool ? "true" : "false")); return 1; } - warnx("PASS: decoder: bool1"); + PX4_INFO("PASS: decoder: bool1"); return 1; } if (!strcmp(node->name, "int1")) { if (node->type != BSON_INT32) { - warnx("FAIL: decoder: int1 type %d, expected %d", node->type, BSON_INT32); + PX4_ERR("FAIL: decoder: int1 type %d, expected %d", node->type, BSON_INT32); return 1; } if (node->i != sample_small_int) { - warnx("FAIL: decoder: int1 value %lld, expected %d", node->i, sample_small_int); + PX4_ERR("FAIL: decoder: int1 value %" PRIu64 ", expected %d", node->i, sample_small_int); return 1; } @@ -126,12 +137,12 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) if (!strcmp(node->name, "int2")) { if (node->type != BSON_INT64) { - warnx("FAIL: decoder: int2 type %d, expected %d", node->type, BSON_INT64); + PX4_ERR("FAIL: decoder: int2 type %d, expected %d", node->type, BSON_INT64); return 1; } if (node->i != sample_big_int) { - warnx("FAIL: decoder: int2 value %lld, expected %lld", node->i, sample_big_int); + PX4_ERR("FAIL: decoder: int2 value %" PRIu64 ", expected %" PRIu64, node->i, sample_big_int); return 1; } @@ -141,12 +152,12 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) if (!strcmp(node->name, "double1")) { if (node->type != BSON_DOUBLE) { - warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE); + PX4_ERR("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE); return 1; } if (fabs(node->d - sample_double) > 1e-12) { - warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double); + PX4_ERR("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double); return 1; } @@ -156,36 +167,36 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) if (!strcmp(node->name, "string1")) { if (node->type != BSON_STRING) { - warnx("FAIL: decoder: string1 type %d, expected %d", node->type, BSON_STRING); + PX4_ERR("FAIL: decoder: string1 type %d, expected %d", node->type, BSON_STRING); return 1; } len = bson_decoder_data_pending(decoder); if (len != strlen(sample_string) + 1) { - warnx("FAIL: decoder: string1 length %d wrong, expected %d", len, strlen(sample_string) + 1); + PX4_ERR("FAIL: decoder: string1 length %d wrong, expected %zd", len, strlen(sample_string) + 1); return 1; } char sbuf[len]; if (bson_decoder_copy_data(decoder, sbuf)) { - warnx("FAIL: decoder: string1 copy failed"); + PX4_ERR("FAIL: decoder: string1 copy failed"); return 1; } if (bson_decoder_data_pending(decoder) != 0) { - warnx("FAIL: decoder: string1 copy did not exhaust all data"); + PX4_ERR("FAIL: decoder: string1 copy did not exhaust all data"); return 1; } if (sbuf[len - 1] != '\0') { - warnx("FAIL: decoder: string1 not 0-terminated"); + PX4_ERR("FAIL: decoder: string1 not 0-terminated"); return 1; } if (strcmp(sbuf, sample_string)) { - warnx("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, sample_string); + PX4_ERR("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, sample_string); return 1; } @@ -195,45 +206,45 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) if (!strcmp(node->name, "data1")) { if (node->type != BSON_BINDATA) { - warnx("FAIL: decoder: data1 type %d, expected %d", node->type, BSON_BINDATA); + PX4_ERR("FAIL: decoder: data1 type %d, expected %d", node->type, BSON_BINDATA); return 1; } len = bson_decoder_data_pending(decoder); if (len != sizeof(sample_data)) { - warnx("FAIL: decoder: data1 length %d, expected %d", len, sizeof(sample_data)); + PX4_ERR("FAIL: decoder: data1 length %d, expected %zu", len, sizeof(sample_data)); return 1; } if (node->subtype != BSON_BIN_BINARY) { - warnx("FAIL: decoder: data1 subtype %d, expected %d", node->subtype, BSON_BIN_BINARY); + PX4_ERR("FAIL: decoder: data1 subtype %d, expected %d", node->subtype, BSON_BIN_BINARY); return 1; } uint8_t dbuf[len]; if (bson_decoder_copy_data(decoder, dbuf)) { - warnx("FAIL: decoder: data1 copy failed"); + PX4_ERR("FAIL: decoder: data1 copy failed"); return 1; } if (bson_decoder_data_pending(decoder) != 0) { - warnx("FAIL: decoder: data1 copy did not exhaust all data"); + PX4_ERR("FAIL: decoder: data1 copy did not exhaust all data"); return 1; } if (memcmp(sample_data, dbuf, len)) { - warnx("FAIL: decoder: data1 compare fail"); + PX4_ERR("FAIL: decoder: data1 compare fail"); return 1; } - warnx("PASS: decoder: data1"); + PX4_INFO("PASS: decoder: data1"); return 1; } if (node->type != BSON_EOO) { - warnx("FAIL: decoder: unexpected node name '%s'", node->name); + PX4_ERR("FAIL: decoder: unexpected node name '%s'", node->name); } return 1; @@ -259,29 +270,33 @@ test_bson(int argc, char *argv[]) /* encode data to a memory buffer */ if (bson_encoder_init_buf(&encoder, NULL, 0)) { - errx(1, "FAIL: bson_encoder_init_buf"); + PX4_ERR("FAIL: bson_encoder_init_buf"); + return 1; } encode(&encoder); len = bson_encoder_buf_size(&encoder); if (len <= 0) { - errx(1, "FAIL: bson_encoder_buf_len"); + PX4_ERR("FAIL: bson_encoder_buf_len"); + return 1; } buf = bson_encoder_buf_data(&encoder); if (buf == NULL) { - errx(1, "FAIL: bson_encoder_buf_data"); + PX4_ERR("FAIL: bson_encoder_buf_data"); + return 1; } /* now test-decode it */ if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL)) { - errx(1, "FAIL: bson_decoder_init_buf"); + PX4_ERR("FAIL: bson_decoder_init_buf"); + return 1; } decode(&decoder); free(buf); - return OK; -} \ No newline at end of file + return PX4_OK; +} diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 180c3f103265..e451755959e9 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -59,20 +59,20 @@ int test_conv(int argc, char *argv[]) { - warnx("Testing system conversions"); + PX4_INFO("Testing system conversions"); for (int i = -10000; i <= 10000; i += 1) { float f = i / 10000.0f; float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); if (fabsf(f - fres) > 0.0001f) { - warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), - (double)fres); + PX4_ERR("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), + (double)fres); return 1; } } - warnx("All conversions clean"); + PX4_INFO("All conversions clean"); return 0; } diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index da73f6896436..3478077b218a 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -59,7 +59,7 @@ #include "dataman/dataman.h" -static sem_t *sems; +static px4_sem_t *sems; static int task_main(int argc, char *argv[]) @@ -137,12 +137,12 @@ task_main(int argc, char *argv[]) rend = hrt_absolute_time(); warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.", my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000); - sem_post(sems + my_id); + px4_sem_post(sems + my_id); return 0; fail: warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x", my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); - sem_post(sems + my_id); + px4_sem_post(sems + my_id); return -1; } @@ -155,7 +155,7 @@ int test_dataman(int argc, char *argv[]) num_tasks = atoi(argv[1]); } - sems = (sem_t *)malloc(num_tasks * sizeof(sem_t)); + sems = (px4_sem_t *)malloc(num_tasks * sizeof(px4_sem_t)); warnx("Running %d tasks", num_tasks); for (i = 0; i < num_tasks; i++) { @@ -165,7 +165,7 @@ int test_dataman(int argc, char *argv[]) const char *av[2]; av[0] = a; av[1] = 0; - sem_init(sems + i, 1, 0); + px4_sem_init(sems + i, 1, 0); /* start the task */ if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) { @@ -174,8 +174,8 @@ int test_dataman(int argc, char *argv[]) } for (i = 0; i < num_tasks; i++) { - sem_wait(sems + i); - sem_destroy(sems + i); + px4_sem_wait(sems + i); + px4_sem_destroy(sems + i); } free(sems); diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index 87035059aabd..ba3922784bcc 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -1,44 +1,47 @@ /**************************************************************************** - * - * 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. - * - ****************************************************************************/ +* +* 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 test_eigen.cpp * * Eigen test (based of mathlib test) * @author Johan Jansen + * @author Nuno Marques */ +#include #include +#include #include #include #include @@ -50,38 +53,53 @@ #include "tests.h" -namespace Eigen -{ +using namespace Eigen; + typedef Matrix Vector10f; -} -static constexpr size_t OPERATOR_ITERATIONS = 60000; +static constexpr size_t OPERATOR_ITERATIONS = 30000; + +const float min = -M_PI_F; +const float max = M_PI_F; +const float step = M_PI_F / 12; +const float epsilon_f = 1E-4; +uint8_t err_num; -#define TEST_OP(_title, _op) \ - { \ - const hrt_abstime t0 = hrt_absolute_time(); \ - for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ - _op; \ - } \ - printf(_title ": %.6fus\n", static_cast(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ +#define TEST_OP(_title, _op) \ + { \ + const hrt_abstime t0 = hrt_absolute_time(); \ + for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ + _op; \ + } \ + printf("%llu: %s: finished in: %.6fus\n", (unsigned long long)t0, _title, static_cast(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ } -#define VERIFY_OP(_title, _op, __OP_TEST__) \ - { \ - _op; \ - if(!(__OP_TEST__)) { \ - printf(_title " Failed! ("#__OP_TEST__")\n"); \ +#define VERIFY_OP(_title, _op, __OP_TEST__) \ + { \ + _op; \ + if (!(__OP_TEST__)) { \ + printf(_title ": Failed! (" # __OP_TEST__ ")\n"); \ + ++err_num; \ } \ } -#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \ - VERIFY_OP(_title, _op, __OP_TEST__) \ +#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \ + VERIFY_OP(_title, _op, __OP_TEST__) \ TEST_OP(_title, _op) +#define EXPECT_QUATERNION(q_exp, q_act, epsilon) \ + (fabsf(q_exp.x() - q_act.x()) <= epsilon && \ + fabsf(q_exp.y() - q_act.y()) <= epsilon && \ + fabsf(q_exp.z() - q_act.z()) <= epsilon && \ + fabsf(q_exp.w() - q_act.w()) <= epsilon) + +#define EXPECT_NEAR(expected, actual, epsilon) \ + (fabsf(expected - actual) <= epsilon) + /** -* @brief -* Prints an Eigen::Matrix to stdout -**/ + * @brief + * Prints an Eigen::Matrix to stdout + **/ template void printEigen(const Eigen::MatrixBase &b) { @@ -100,24 +118,72 @@ void printEigen(const Eigen::MatrixBase &b) } } +// Methods definition +Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy); +Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q); +Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy); +Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q); +math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q); + /** -* @brief -* Construct new Eigen::Quaternion from euler angles -**/ -template -Eigen::Quaternion quatFromEuler(const T roll, const T pitch, const T yaw) + * @brief + * Construct new Eigen::Quaternion from euler angles + * Right order is YPR. + **/ +Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy) +{ + return Eigen::Quaternionf( + Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) * + Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) * + Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX()) + ); +} + +/** + * @brief + * Construct new Eigen::Vector3f of euler angles from quaternion + * Right order is YPR. + **/ +Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q) +{ + return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse(); +} + +/** + * @brief + * Construct new Eigen::Matrix3f from euler angles + **/ +Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy) { - Eigen::AngleAxis rollAngle(roll, Eigen::Matrix::UnitZ()); - Eigen::AngleAxis yawAngle(yaw, Eigen::Matrix::UnitY()); - Eigen::AngleAxis pitchAngle(pitch, Eigen::Matrix::UnitX()); - return rollAngle * yawAngle * pitchAngle; + return quatFromEuler(rpy).toRotationMatrix(); } +/** + * @brief + * Adjust PX4 math::quaternion to Eigen::Quaternionf + **/ +Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q) +{ + return Eigen::Quaternionf(q.data[1], q.data[2], q.data[3], q.data[0]); +} +/** + * @brief + * Adjust Eigen::Quaternionf to PX4 math::quaternion + **/ +math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q) +{ + return math::Quaternion(q.w(), q.x(), q.y(), q.z()); +} + +/** + * @brief + * Testing main routine + **/ int test_eigen(int argc, char *argv[]) { int rc = 0; - warnx("testing eigen"); + warnx("Testing Eigen math..."); { Eigen::Vector2f v; @@ -134,7 +200,6 @@ int test_eigen(int argc, char *argv[]) VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1)); VERIFY_OP("Vector2f -= Vector2f", v -= v1, v.isApprox(v1)); TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON); - //TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array? } { @@ -189,12 +254,12 @@ int test_eigen(int argc, char *argv[]) } { - Eigen::Vector10f v1; + Vector10f v1; v1.Zero(); float data[10]; - TEST_OP("Constructor Vector<10>()", Eigen::Vector10f v3); - TEST_OP("Constructor Vector<10>(Vector<10>)", Eigen::Vector10f v3(v1)); - TEST_OP("Constructor Vector<10>(float[])", Eigen::Vector10f v3(data)); + TEST_OP("Constructor Vector<10>()", Vector10f v3); + TEST_OP("Constructor Vector<10>(Vector<10>)", Vector10f v3(v1)); + TEST_OP("Constructor Vector<10>(float[])", Vector10f v3(data)); } { @@ -213,7 +278,7 @@ int test_eigen(int argc, char *argv[]) m1.setIdentity(); Eigen::Matrix m2; m2.setIdentity(); - Eigen::Vector10f v1; + Vector10f v1; v1.Zero(); TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1); TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); @@ -221,7 +286,7 @@ int test_eigen(int argc, char *argv[]) } { - warnx("Nonsymmetric matrix operations test"); + printf("%llu: Nonsymmetric matrix operations test...\n", (unsigned long long)hrt_absolute_time()); // test nonsymmetric +, -, +=, -= const Eigen::Matrix m1_orig = @@ -239,194 +304,261 @@ int test_eigen(int argc, char *argv[]) 12, 15, 18; if (m1 + m2 != m3) { - warnx("Matrix<2, 3> + Matrix<2, 3> failed!"); + printf("%llu: Matrix<2, 3> + Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time()); printEigen(m1 + m2); printf("!=\n"); printEigen(m3); + ++err_num; rc = 1; } if (m3 - m2 != m1) { - warnx("Matrix<2, 3> - Matrix<2, 3> failed!"); + printf("%llu: Matrix<2, 3> - Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time()); printEigen(m3 - m2); printf("!=\n"); printEigen(m1); + ++err_num; rc = 1; } m1 += m2; if (m1 != m3) { - warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); + printf("%llu: Matrix<2, 3> += Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time()); printEigen(m1); printf("!=\n"); printEigen(m3); + ++err_num; rc = 1; } m1 -= m2; if (m1 != m1_orig) { - warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); + printf("%llu: Matrix<2, 3> -= Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time()); printEigen(m1); printf("!=\n"); printEigen(m1_orig); + ++err_num; rc = 1; } - } - /* QUATERNION TESTS CURRENTLY FAILING + /* QUATERNION TESTS */ { - // test conversion rotation matrix to quaternion and back - Eigen::Matrix3f R_orig; - Eigen::Matrix3f R; + // Test conversion rotation matrix to quaternion and back + // against existing PX4 mathlib + math::Matrix<3, 3> R_orig; + Eigen::Quaternionf q_true; Eigen::Quaternionf q; - float diff = 0.1f; - float tol = 0.00001f; + Eigen::Matrix3f R; - warnx("Quaternion transformation methods test."); + printf("%llu: Conversion method: Quaternion transformation methods test...\n", (unsigned long long)hrt_absolute_time()); + printf("%llu: Conversion method: Testing known values...\n", (unsigned long long)hrt_absolute_time()); - for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { - for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { - for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { - R_orig.eulerAngles(roll, pitch, yaw); - q = R_orig; //from_dcm - R = q.toRotationMatrix(); + /******************************************** TEST 1 ****************************************************/ + q_true = {0.0f, 0.0f, 0.0f, 1.0f}; + math::Quaternion q_px4 = {1.0f, 0.0f, 0.0f, 0.0f}; + Eigen::Quaternionf q_eigen(eigenqFromPx4q(q_px4)); - for (size_t i = 0; i < 3; i++) { - for (size_t j = 0; j < 3; j++) { - if (fabsf(R_orig(i, j) - R(i, j)) > tol) { - warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!"); - rc = 1; - } - } - } - } - } + if (!EXPECT_QUATERNION(q_true, q_eigen, FLT_EPSILON)) { + printf("%llu: Value of: Quaternion1 [1.0, 0.0, 0.0, 0.0]\n", (unsigned long long)hrt_absolute_time()); + printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w()); + printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w()); + ++err_num; + rc = 1; + } + + /********************************************************************************************************/ + /******************************************** TEST 2 ****************************************************/ + q_true = {1.0f, 0.0f, 0.0f, 0.0f}; + Eigen::Quaternionf q2_eigen = {0.0f, 0.0f, 0.0f, 1.0f}; + math::Quaternion q2_px4(px4qFromEigenq(q2_eigen)); + Eigen::Quaternionf q2_eigen_(q2_px4.data[3], q2_px4.data[0], q2_px4.data[1], q2_px4.data[2]); + + if (!EXPECT_QUATERNION(q_true, q2_eigen_, FLT_EPSILON)) { + printf("%llu: Value of: Quaternion2 [0.0, 0.0, 0.0, 1.0]\n", (unsigned long long)hrt_absolute_time()); + printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q2_px4.data[0], q2_px4.data[1], q2_px4.data[2], q2_px4.data[3]); + printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w()); + ++err_num; + rc = 1; } - // test against some known values - tol = 0.0001f; - Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f}; - R_orig.setIdentity(); - q = R_orig; + /********************************************************************************************************/ + /******************************************** TEST 3 ****************************************************/ + q_true = quatFromEuler(Eigen::Vector3f(0.3f, 0.2f, 0.1f)); + q = {0.9833474432563558f, 0.14357217502739184f, 0.10602051106179561f, 0.0342707985504821f}; - for (size_t i = 0; i < 4; i++) { - if (!q.isApprox(q_true, tol)) { - warnx("Quaternion method 'from_dcm()' outside tolerance!"); - rc = 1; - } + if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) { + printf("%llu: Value of: Quaternion [0.9833, 0.1436, 0.1060, 0.0343]\n", (unsigned long long)hrt_absolute_time()); + printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z()); + printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z()); + ++err_num; + rc = 1; } - q_true = quatFromEuler(0.3f, 0.2f, 0.1f); - q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; + /********************************************************************************************************/ + /******************************************** TEST 4 ****************************************************/ + q_true = quatFromEuler(Eigen::Vector3f(-1.5f, -0.2f, 0.5f)); + q = {0.7222365948153096f, -0.6390766544101811f, -0.2385737751841646f, 0.11418355701173476f}; - for (size_t i = 0; i < 4; i++) { - if (!q.isApprox(q_true, tol)) { - warnx("Quaternion method 'eulerAngles()' outside tolerance!"); - rc = 1; - } + if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) { + printf("%llu: Value of: Quaternion [0.7222, -0.6391, -0.2386, 0.1142]\n", (unsigned long long)hrt_absolute_time()); + printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z()); + printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z()); + ++err_num; + rc = 1; } - q_true = quatFromEuler(-1.5f, -0.2f, 0.5f); - q = {0.7222f, -0.6391f, -0.2386f, 0.1142f}; + /********************************************************************************************************/ + /******************************************** TEST 5 ****************************************************/ + q_true = quatFromEuler(Eigen::Vector3f(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3)); + q = {0.6830127018922193f, 0.18301270189221933f, -0.6830127018922193f, 0.18301270189221933f}; for (size_t i = 0; i < 4; i++) { - if (!q.isApprox(q_true, tol)) { - warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) { + printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n", + (unsigned long long)hrt_absolute_time(), (int)i); + printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z()); + printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z()); + ++err_num; rc = 1; } } - q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); - q = {0.6830f, 0.1830f, -0.6830f, 0.1830f}; + /********************************************************************************************************/ + /******************************************** TEST 6 ****************************************************/ + printf("%llu: Conversion method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time()); - for (size_t i = 0; i < 4; i++) { - if (!q.isApprox(q_true, tol)) { - warnx("Quaternion method 'eulerAngles()' outside tolerance!"); - rc = 1; + for (float roll = min; roll <= max; roll += step) { + for (float pitch = min; pitch <= max; pitch += step) { + for (float yaw = min; yaw <= max; yaw += step) { + + q = Eigen::Quaternionf(quatFromEuler(Eigen::Vector3f(roll, pitch, yaw))); + + R = q.toRotationMatrix(); + R_orig.from_euler(roll, pitch, yaw); + + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + if (!EXPECT_NEAR(R_orig(i, j), R(i, j), epsilon_f)) { + printf("%llu: (%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n", + (unsigned long long)hrt_absolute_time(), (int)i, (int)j); + printf("Actual: \t%8.5f\n", R(i, j)); + printf("Expected: \t%8.5f\n", R_orig(i, j)); + ++err_num; + rc = 1; + } + } + } + } } } - } { - // test quaternion method "rotate" (rotate vector by quaternion) + // Test rotation method (rotate vector by quaternion) Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f}; Eigen::Vector3f vector_q; Eigen::Vector3f vector_r; Eigen::Quaternionf q; Eigen::Matrix3f R; - float diff = 0.1f; - float tol = 0.00001f; - warnx("Quaternion vector rotation method test."); + printf("%llu: Rotation method: Quaternion vector rotation method test...\n", (unsigned long long)hrt_absolute_time()); + printf("%llu: Rotation method: Testing known values...\n", (unsigned long long)hrt_absolute_time()); - for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { - for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { - for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { - R.eulerAngles(roll, pitch, yaw); - q = quatFromEuler(roll, pitch, yaw); - vector_r = R * vector; - vector_q = q._transformVector(vector); + /******************************************** TEST 1 ****************************************************/ + q = quatFromEuler(Eigen::Vector3f(0.0f, 0.0f, M_PI_2_F)); + vector_q = q._transformVector(vector); + Eigen::Vector3f vector_true = { -1.00f, 1.00f, 1.00f}; - for (int i = 0; i < 3; i++) { - if (fabsf(vector_r(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); - rc = 1; - } - } - } + for (size_t i = 0; i < 3; i++) { + if (!EXPECT_NEAR(vector_true(i), vector_q(i), FLT_EPSILON)) { + printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i); + printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", vector_q(0), vector_q(1), vector_q(2)); + printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", vector_true(0), vector_true(1), vector_true(2)); + ++err_num; + rc = 1; } } - // test some values calculated with matlab - tol = 0.0001f; - q = quatFromEuler(M_PI_2_F, 0.0f, 0.0f); + /********************************************************************************************************/ + /******************************************** TEST 2 ****************************************************/ + q = quatFromEuler(Eigen::Vector3f(0.1f, 0.2f, 0.3f)); vector_q = q._transformVector(vector); - Eigen::Vector3f vector_true = {1.00f, -1.00f, 1.00f}; + vector_true = {0.8795481794122900f, 1.2090975499501229f, 0.874344391414010f}; for (size_t i = 0; i < 3; i++) { - if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) { + printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i); + printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2)); + printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2)); + ++err_num; rc = 1; } } - q = quatFromEuler(0.3f, 0.2f, 0.1f); + /********************************************************************************************************/ + /******************************************** TEST 3 ****************************************************/ + q = quatFromEuler(Eigen::Vector3f(0.5f, -0.2f, -1.5f)); vector_q = q._transformVector(vector); - vector_true = {1.1566, 0.7792, 1.0273}; + vector_true = {0.447416342848463f, -0.6805264343934600f, 1.528627615949624f}; for (size_t i = 0; i < 3; i++) { - if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) { + printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i); + printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2)); + printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2)); + ++err_num; rc = 1; } } - q = quatFromEuler(-1.5f, -0.2f, 0.5f); + /********************************************************************************************************/ + /******************************************** TEST 4 ****************************************************/ + q = quatFromEuler(Eigen::Vector3f(-M_PI_F / 3.0f, -M_PI_2_F, M_PI_2_F)); vector_q = q._transformVector(vector); - vector_true = {0.5095, 1.4956, -0.7096}; + vector_true = { -1.366030f, 0.366025f, 1.000000f}; for (size_t i = 0; i < 3; i++) { - if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) { + printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i); + printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2)); + printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2)); + ++err_num; rc = 1; } } - q = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f); - vector_q = q._transformVector(vector); - vector_true = { -1.3660, 0.3660, 1.0000}; + /********************************************************************************************************/ + /******************************************** TEST 5 ****************************************************/ + printf("%llu: Rotation method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time()); - for (size_t i = 0; i < 3; i++) { - if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); - rc = 1; + Eigen::Vector3f vectorR(1.0f, 1.0f, 1.0f); + + for (float roll = min; roll <= max; roll += step) { + for (float pitch = min; pitch <= max; pitch += step) { + for (float yaw = min; yaw <= max; yaw += step) { + + R = matrixFromEuler(Eigen::Vector3f(roll, pitch, yaw)); + q = quatFromEuler(Eigen::Vector3f(roll, pitch, yaw)); + + vector_r = R * vectorR; + vector_q = q._transformVector(vectorR); + + for (int i = 0; i < 3; i++) { + if (!EXPECT_NEAR(vector_r(i), vector_q(i), epsilon_f)) { + printf("%llu: (%d) Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), i); + printf("Actual: \t%8.5f\n", vector_q(i)); + printf("Expected: \t%8.5f\n", vector_r(i)); + ++err_num; + rc = 1; + } + } + } } } } - */ + printf("%llu: Finished Eigen math tests with %d error(s)...\n", (unsigned long long)hrt_absolute_time(), err_num); return rc; } diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index a43e01d6fb54..bdb089e40518 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -97,7 +97,7 @@ test_file(int argc, char *argv[]) /* check if microSD card is mounted */ struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { + if (stat(PX4_ROOTFSDIR "/fs/microsd/", &buffer)) { warnx("no microSD card mounted, aborting file test"); return 1; } @@ -125,7 +125,7 @@ test_file(int argc, char *argv[]) uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); hrt_abstime start, end; - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + int fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); warnx("testing unaligned writes - please wait.."); @@ -154,10 +154,10 @@ test_file(int argc, char *argv[]) end = hrt_absolute_time(); - warnx("write took %llu us", (end - start)); + warnx("write took %" PRIu64 " us", (end - start)); close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); + fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY); /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { @@ -195,8 +195,8 @@ test_file(int argc, char *argv[]) */ close(fd); - int ret = unlink("/fs/microsd/testfile"); - fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + int ret = unlink(PX4_ROOTFSDIR "/fs/microsd/testfile"); + fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); @@ -219,7 +219,7 @@ test_file(int argc, char *argv[]) warnx("reading data aligned.."); close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); + fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY); bool align_read_ok = true; @@ -256,7 +256,7 @@ test_file(int argc, char *argv[]) warnx("reading data unaligned.."); close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); + fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY); bool unalign_read_ok = true; int unalign_read_err_count = 0; @@ -297,7 +297,7 @@ test_file(int argc, char *argv[]) } - ret = unlink("/fs/microsd/testfile"); + ret = unlink(PX4_ROOTFSDIR "/fs/microsd/testfile"); close(fd); if (ret) { @@ -310,7 +310,7 @@ test_file(int argc, char *argv[]) /* list directory */ DIR *d; struct dirent *dir; - d = opendir("/fs/microsd"); + d = opendir(PX4_ROOTFSDIR "/fs/microsd"); if (d) { diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index 2e7b5c184fae..56d9f0d83ab2 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -37,17 +37,17 @@ * File write test. */ +#include #include #include #include #include #include #include -#include #include #include #include -#include +#include #include "tests.h" @@ -73,7 +73,7 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t filename, (unsigned)write_chunk, (unsigned)write_size); uint32_t ofs = 0; - int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC); + int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC, PX4_O_MODE_666); if (fd == -1) { perror(filename); @@ -133,13 +133,13 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) { printf("read failed at offset %u\n", ofs); - exit(1); + return; } for (uint16_t j = 0; j < write_chunk; j++) { if (buffer[j] != get_value(ofs)) { printf("corruption at ofs=%u got %u\n", ofs, buffer[j]); - exit(1); + return; } ofs++; @@ -170,11 +170,14 @@ int test_file2(int argc, char *argv[]) { int opt; uint16_t flags = 0; - const char *filename = "/fs/microsd/testfile2.dat"; + const char *filename = PX4_ROOTFSDIR "/fs/microsd/testfile2.dat"; uint32_t write_chunk = 64; uint32_t write_size = 5 * 1024; - while ((opt = getopt(argc, argv, "c:s:FLh")) != EOF) { + int myoptind = 1; + const char *myoptarg = NULL; + + while ((opt = px4_getopt(argc, argv, "c:s:FLh", &myoptind, &myoptarg)) != EOF) { switch (opt) { case 'F': flags |= FLAG_FSYNC; @@ -185,22 +188,22 @@ int test_file2(int argc, char *argv[]) break; case 's': - write_size = strtoul(optarg, NULL, 0); + write_size = strtoul(myoptarg, NULL, 0); break; case 'c': - write_chunk = strtoul(optarg, NULL, 0); + write_chunk = strtoul(myoptarg, NULL, 0); break; case 'h': default: usage(); - exit(1); + return 1; } } - argc -= optind; - argv += optind; + argc -= myoptind; + argv += myoptind; if (argc > 0) { filename = argv[0]; @@ -209,8 +212,8 @@ int test_file2(int argc, char *argv[]) /* check if microSD card is mounted */ struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { - warnx("no microSD card mounted, aborting file test"); + if (stat(PX4_ROOTFSDIR "/fs/microsd/", &buffer)) { + fprintf(stderr, "no microSD card mounted, aborting file test"); return 1; } diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index 55f466e7343a..83b102b41e89 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -39,12 +39,12 @@ #include #include +#include #include #include #include #include #include -#include #include "tests.h" #include #include diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c index e14b4b4e6c51..3d27d903f0fc 100644 --- a/src/systemcmds/tests/test_gpio.c +++ b/src/systemcmds/tests/test_gpio.c @@ -37,6 +37,7 @@ ****************************************************************************/ #include +#include #include @@ -45,9 +46,8 @@ #include #include #include -#include -#include +#include #include "tests.h" @@ -93,7 +93,7 @@ int test_gpio(int argc, char *argv[]) #ifdef PX4IO_DEVICE_PATH - int fd = open(PX4IO_DEVICE_PATH, 0); + int fd = px4_open(PX4IO_DEVICE_PATH, 0); if (fd < 0) { printf("GPIO: open fail\n"); @@ -101,16 +101,16 @@ int test_gpio(int argc, char *argv[]) } /* set all GPIOs to default state */ - ioctl(fd, GPIO_RESET, ~0); + px4_ioctl(fd, GPIO_RESET, ~0); /* XXX need to add some GPIO waving stuff here */ /* Go back to default */ - ioctl(fd, GPIO_RESET, ~0); + px4_ioctl(fd, GPIO_RESET, ~0); - close(fd); + px4_close(fd); printf("\t GPIO test successful.\n"); #endif diff --git a/src/systemcmds/tests/test_hott_telemetry.c b/src/systemcmds/tests/test_hott_telemetry.c index 281e7e5029bb..d1e928119108 100644 --- a/src/systemcmds/tests/test_hott_telemetry.c +++ b/src/systemcmds/tests/test_hott_telemetry.c @@ -45,10 +45,11 @@ #include #include +#include +#include #include #include -#include #include #include #include @@ -92,7 +93,7 @@ static int open_uart(const char *device) int uart = open(device, O_RDWR | O_NOCTTY); if (uart < 0) { - errx(1, "FAIL: Error opening port"); + PX4_ERR("FAIL: Error opening port"); return ERROR; } @@ -107,12 +108,12 @@ static int open_uart(const char *device) /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - errx(1, "FAIL: Error setting baudrate / termios config for cfsetispeed, cfsetospeed"); + PX4_ERR("FAIL: Error setting baudrate / termios config for cfsetispeed, cfsetospeed"); return ERROR; } if (tcsetattr(uart, TCSANOW, &uart_config) < 0) { - errx(1, "FAIL: Error setting baudrate / termios config for tcsetattr"); + PX4_ERR("FAIL: Error setting baudrate / termios config for tcsetattr"); return ERROR; } @@ -129,11 +130,11 @@ static int open_uart(const char *device) int test_hott_telemetry(int argc, char *argv[]) { - warnx("HoTT Telemetry Test Requirements:"); - warnx("- Radio on and Electric Air. Mod on (telemetry -> sensor select)."); - warnx("- Receiver telemetry port must be in telemetry mode."); - warnx("- Connect telemetry wire to /dev/ttyS1 (USART2)."); - warnx("Testing..."); + PX4_INFO("HoTT Telemetry Test Requirements:"); + PX4_INFO("- Radio on and Electric Air. Mod on (telemetry -> sensor select)."); + PX4_INFO("- Receiver telemetry port must be in telemetry mode."); + PX4_INFO("- Connect telemetry wire to /dev/ttyS1 (USART2)."); + PX4_INFO("Testing..."); const char device[] = "/dev/ttyS1"; int fd = open_uart(device); @@ -143,8 +144,10 @@ int test_hott_telemetry(int argc, char *argv[]) return ERROR; } +#ifdef TIOCSSINGLEWIRE /* Activate single wire mode */ ioctl(fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); +#endif char send = 'a'; write(fd, &send, 1); @@ -154,12 +157,13 @@ int test_hott_telemetry(int argc, char *argv[]) struct pollfd fds[] = { { .fd = fd, .events = POLLIN } }; if (poll(fds, 1, timeout) == 0) { - errx(1, "FAIL: Could not read sent data."); + PX4_ERR("FAIL: Could not read sent data."); + return 1; } char receive; read(fd, &receive, 1); - warnx("PASS: Single wire enabled. Sent %x and received %x", send, receive); + PX4_INFO("PASS: Single wire enabled. Sent %x and received %x", send, receive); /* Attempt to read HoTT poll messages from the HoTT receiver */ @@ -170,8 +174,8 @@ int test_hott_telemetry(int argc, char *argv[]) for (; received_count < 5; received_count++) { if (poll(fds, 1, timeout) == 0) { - errx(1, "FAIL: Could not read sent data. Is your HoTT receiver plugged in on %s?", device); - break; + PX4_ERR("FAIL: Could not read sent data. Is your HoTT receiver plugged in on %s?", device); + return 1; } else { read(fd, &byte, 1); @@ -187,21 +191,23 @@ int test_hott_telemetry(int argc, char *argv[]) if (received_count > 0 && valid_count > 0) { if (received_count == max_polls && valid_count == max_polls) { - warnx("PASS: Received %d out of %d valid byte pairs from the HoTT receiver device.", received_count, max_polls); + PX4_INFO("PASS: Received %d out of %d valid byte pairs from the HoTT receiver device.", received_count, max_polls); } else { - warnx("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count, - max_polls, valid_count); + PX4_WARN("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count, + max_polls, valid_count); } } else { /* Let's work out what went wrong */ if (received_count == 0) { - errx(1, "FAIL: Could not read any polls from HoTT receiver device."); + PX4_ERR("FAIL: Could not read any polls from HoTT receiver device."); + return 1; } if (valid_count == 0) { - errx(1, "FAIL: Received unexpected values from the HoTT receiver device."); + PX4_ERR("FAIL: Received unexpected values from the HoTT receiver device."); + return 1; } } @@ -221,21 +227,24 @@ int test_hott_telemetry(int argc, char *argv[]) usleep(1000); } - warnx("PASS: Response sent to the HoTT receiver device. Voltage should now show 2.5V."); + PX4_INFO("PASS: Response sent to the HoTT receiver device. Voltage should now show 2.5V."); +#ifdef TIOCSSINGLEWIRE /* Disable single wire */ ioctl(fd, TIOCSSINGLEWIRE, ~SER_SINGLEWIRE_ENABLED); +#endif write(fd, &send, 1); /* We should timeout as there will be nothing to read (TX and RX no longer connected) */ if (poll(fds, 1, timeout) == 0) { - errx(1, "FAIL: timeout expected."); + PX4_ERR("FAIL: timeout expected."); + return 1; } - warnx("PASS: Single wire disabled."); + PX4_INFO("PASS: Single wire disabled."); close(fd); - exit(0); + return 0; } diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index 40043c69be8a..8bc088812234 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -46,7 +46,6 @@ #include #include #include -#include #include #include @@ -54,7 +53,7 @@ #include #include -#include +//#include #include "tests.h" @@ -127,7 +126,7 @@ int test_tone(int argc, char *argv[]) int fd, result; unsigned long tone; - fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY); + fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); if (fd < 0) { printf("failed opening " TONEALARM0_DEVICE_PATH "\n"); @@ -141,7 +140,7 @@ int test_tone(int argc, char *argv[]) } if (tone == 0) { - result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); + result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); @@ -152,14 +151,14 @@ int test_tone(int argc, char *argv[]) } } else { - result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); + result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); goto out; } - result = ioctl(fd, TONE_SET_ALARM, tone); + result = px4_ioctl(fd, TONE_SET_ALARM, tone); if (result < 0) { printf("failed setting alarm %lu\n", tone); @@ -172,7 +171,7 @@ int test_tone(int argc, char *argv[]) out: if (fd >= 0) { - close(fd); + px4_close(fd); } return 0; diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c index 95ee7652dd54..01092aa2d486 100644 --- a/src/systemcmds/tests/test_int.c +++ b/src/systemcmds/tests/test_int.c @@ -36,16 +36,20 @@ * Included Files ****************************************************************************/ +#define __STDC_FORMAT_MACROS +#include + #include +#include #include #include #include +#include #include #include #include -#include #include @@ -105,10 +109,10 @@ int test_int(int argc, char *argv[]) int64_t calc = large * 5; if (calc == 1770781647990) { - printf("\t success: 354156329598 * 5 == %lld\n", calc); + printf("\t success: 354156329598 * 5 == %" PRId64 "\n", calc); } else { - printf("\t FAIL: 354156329598 * 5 != %lld\n", calc); + printf("\t FAIL: 354156329598 * 5 != %" PRId64 "\n", calc); ret = -1; } @@ -127,10 +131,10 @@ int test_int(int argc, char *argv[]) uint64_t small_times_large = large_int * (uint64_t)small; if (small_times_large == 107374182350) { - printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %lld\n", small_times_large); + printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %" PRId64 "\n", small_times_large); } else { - printf("\t FAIL: 50 * 2147483647 != %lld, 64bit cast might fail\n", small_times_large); + printf("\t FAIL: 50 * 2147483647 != %" PRId64 ", 64bit cast might fail\n", small_times_large); ret = -1; } diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c index a04aacc3a734..f94caa87b3d6 100644 --- a/src/systemcmds/tests/test_jig_voltages.c +++ b/src/systemcmds/tests/test_jig_voltages.c @@ -36,7 +36,7 @@ ****************************************************************************/ #include -#include +#include #include @@ -45,13 +45,12 @@ #include #include #include -#include -#include +//#include #include "tests.h" -#include +#include #include #include diff --git a/src/systemcmds/tests/test_led.c b/src/systemcmds/tests/test_led.c index f56660b74af7..f045640453bd 100644 --- a/src/systemcmds/tests/test_led.c +++ b/src/systemcmds/tests/test_led.c @@ -37,6 +37,7 @@ ****************************************************************************/ #include +#include #include @@ -45,7 +46,6 @@ #include #include #include -#include #include @@ -91,15 +91,15 @@ int test_led(int argc, char *argv[]) int fd; int ret = 0; - fd = open(LED0_DEVICE_PATH, 0); + fd = px4_open(LED0_DEVICE_PATH, 0); if (fd < 0) { printf("\tLED: open fail\n"); return ERROR; } - if (ioctl(fd, LED_ON, LED_BLUE) || - ioctl(fd, LED_ON, LED_AMBER)) { + if (px4_ioctl(fd, LED_ON, LED_BLUE) || + px4_ioctl(fd, LED_ON, LED_AMBER)) { printf("\tLED: ioctl fail\n"); return ERROR; @@ -112,12 +112,12 @@ int test_led(int argc, char *argv[]) for (i = 0; i < 10; i++) { if (ledon) { - ioctl(fd, LED_ON, LED_BLUE); - ioctl(fd, LED_OFF, LED_AMBER); + px4_ioctl(fd, LED_ON, LED_BLUE); + px4_ioctl(fd, LED_OFF, LED_AMBER); } else { - ioctl(fd, LED_OFF, LED_BLUE); - ioctl(fd, LED_ON, LED_AMBER); + px4_ioctl(fd, LED_OFF, LED_BLUE); + px4_ioctl(fd, LED_ON, LED_AMBER); } ledon = !ledon; @@ -125,8 +125,8 @@ int test_led(int argc, char *argv[]) } /* Go back to default */ - ioctl(fd, LED_ON, LED_BLUE); - ioctl(fd, LED_OFF, LED_AMBER); + px4_ioctl(fd, LED_ON, LED_BLUE); + px4_ioctl(fd, LED_OFF, LED_AMBER); printf("\t LED test completed, no errors.\n"); diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 7460f6f5592f..dcd03e1be980 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -37,6 +37,7 @@ * Mathlib test */ +#include #include #include #include @@ -48,14 +49,14 @@ #include "tests.h" -#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); } +#define TEST_OP(_title, _op) { unsigned int n = 30000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); } using namespace math; int test_mathlib(int argc, char *argv[]) { int rc = 0; - warnx("testing mathlib"); + PX4_INFO("testing mathlib"); { Vector<2> v; @@ -158,7 +159,7 @@ int test_mathlib(int argc, char *argv[]) } { - warnx("Nonsymmetric matrix operations test"); + PX4_INFO("Nonsymmetric matrix operations test"); // test nonsymmetric +, -, +=, -= float data1[2][3] = {{1, 2, 3}, {4, 5, 6}}; @@ -170,7 +171,7 @@ int test_mathlib(int argc, char *argv[]) Matrix<2, 3> m3(data3); if (m1 + m2 != m3) { - warnx("Matrix<2, 3> + Matrix<2, 3> failed!"); + PX4_ERR("Matrix<2, 3> + Matrix<2, 3> failed!"); (m1 + m2).print(); printf("!=\n"); m3.print(); @@ -178,7 +179,7 @@ int test_mathlib(int argc, char *argv[]) } if (m3 - m2 != m1) { - warnx("Matrix<2, 3> - Matrix<2, 3> failed!"); + PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!"); (m3 - m2).print(); printf("!=\n"); m1.print(); @@ -188,7 +189,7 @@ int test_mathlib(int argc, char *argv[]) m1 += m2; if (m1 != m3) { - warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); + PX4_ERR("Matrix<2, 3> += Matrix<2, 3> failed!"); m1.print(); printf("!=\n"); m3.print(); @@ -199,7 +200,7 @@ int test_mathlib(int argc, char *argv[]) Matrix<2, 3> m1_orig(data1); if (m1 != m1_orig) { - warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); + PX4_ERR("Matrix<2, 3> -= Matrix<2, 3> failed!"); m1.print(); printf("!=\n"); m1_orig.print(); @@ -216,7 +217,7 @@ int test_mathlib(int argc, char *argv[]) float diff = 0.1f; float tol = 0.00001f; - warnx("Quaternion transformation methods test."); + PX4_INFO("Quaternion transformation methods test."); for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { @@ -228,7 +229,7 @@ int test_mathlib(int argc, char *argv[]) for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { if (fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) { - warnx("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!"); + PX4_WARN("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!"); rc = 1; } } @@ -245,7 +246,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 4; i++) { if (fabsf(q.data[i] - q_true.data[i]) > tol) { - warnx("Quaternion method 'from_dcm()' outside tolerance!"); + PX4_WARN("Quaternion method 'from_dcm()' outside tolerance!"); rc = 1; } } @@ -255,7 +256,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 4; i++) { if (fabsf(q.data[i] - q_true.data[i]) > tol) { - warnx("Quaternion method 'from_euler()' outside tolerance!"); + PX4_WARN("Quaternion method 'from_euler()' outside tolerance!"); rc = 1; } } @@ -265,7 +266,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 4; i++) { if (fabsf(q.data[i] - q_true.data[i]) > tol) { - warnx("Quaternion method 'from_euler()' outside tolerance!"); + PX4_WARN("Quaternion method 'from_euler()' outside tolerance!"); rc = 1; } } @@ -275,7 +276,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 4; i++) { if (fabsf(q.data[i] - q_true.data[i]) > tol) { - warnx("Quaternion method 'from_euler()' outside tolerance!"); + PX4_WARN("Quaternion method 'from_euler()' outside tolerance!"); rc = 1; } } @@ -292,7 +293,7 @@ int test_mathlib(int argc, char *argv[]) float diff = 0.1f; float tol = 0.00001f; - warnx("Quaternion vector rotation method test."); + PX4_INFO("Quaternion vector rotation method test."); for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { @@ -304,7 +305,7 @@ int test_mathlib(int argc, char *argv[]) for (int i = 0; i < 3; i++) { if (fabsf(vector_r(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + PX4_WARN("Quaternion method 'rotate' outside tolerance"); rc = 1; } } @@ -320,7 +321,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + PX4_WARN("Quaternion method 'rotate' outside tolerance"); rc = 1; } } @@ -331,7 +332,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + PX4_WARN("Quaternion method 'rotate' outside tolerance"); rc = 1; } } @@ -342,7 +343,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + PX4_WARN("Quaternion method 'rotate' outside tolerance"); rc = 1; } } @@ -353,7 +354,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + PX4_WARN("Quaternion method 'rotate' outside tolerance"); rc = 1; } } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index d0f76d3b69e5..98b65f10d1e1 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -56,6 +56,8 @@ #include #include +#include + #include "tests.h" static int mixer_callback(uintptr_t handle, @@ -65,6 +67,9 @@ static int mixer_callback(uintptr_t handle, const unsigned output_max = 8; static float actuator_controls[output_max]; +static bool should_prearm = false; + +#define NAN_VALUE 0.0f/0.0f int test_mixer(int argc, char *argv[]) { @@ -72,7 +77,7 @@ int test_mixer(int argc, char *argv[]) * PWM limit structure */ pwm_limit_t pwm_limit; - static bool should_arm = false; + bool should_arm = false; uint16_t r_page_servo_disarmed[output_max]; uint16_t r_page_servo_control_min[output_max]; uint16_t r_page_servo_control_max[output_max]; @@ -80,7 +85,7 @@ int test_mixer(int argc, char *argv[]) uint16_t servo_predicted[output_max]; int16_t reverse_pwm_mask = 0; - warnx("testing mixer"); + PX4_INFO("testing mixer"); const char *filename = "/etc/mixers/IO_pass.mix"; @@ -88,14 +93,14 @@ int test_mixer(int argc, char *argv[]) filename = argv[1]; } - warnx("loading: %s", filename); + PX4_INFO("loading: %s", filename); char buf[2048]; load_mixer_file(filename, &buf[0], sizeof(buf)); unsigned loaded = strlen(buf); - warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + PX4_INFO("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); /* load the mixer in chunks, like * in the case of a remote load, @@ -109,7 +114,7 @@ int test_mixer(int argc, char *argv[]) /* load at once test */ unsigned xx = loaded; mixer_group.load_from_buf(&buf[0], xx); - warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count()); if (mixer_group.count() != 8) { return 1; @@ -121,7 +126,7 @@ int test_mixer(int argc, char *argv[]) empty_buf[1] = '\0'; mixer_group.reset(); mixer_group.load_from_buf(&empty_buf[0], empty_load); - warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); + PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); if (empty_load != 0) { return 1; @@ -135,7 +140,7 @@ int test_mixer(int argc, char *argv[]) unsigned transmitted = 0; - warnx("transmitted: %d, loaded: %d", transmitted, loaded); + PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded); while (transmitted < loaded) { @@ -150,7 +155,7 @@ int test_mixer(int argc, char *argv[]) memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; - warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]); + PX4_INFO("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]); /* process the text buffer, adding new mixers as their descriptions can be parsed */ unsigned resid = mixer_text_length; @@ -158,7 +163,7 @@ int test_mixer(int argc, char *argv[]) /* if anything was parsed */ if (resid != mixer_text_length) { - warnx("used %u", mixer_text_length - resid); + PX4_INFO("used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ if (resid > 0) { @@ -171,7 +176,7 @@ int test_mixer(int argc, char *argv[]) transmitted += text_length; } - warnx("chunked load: loaded %u mixers", mixer_group.count()); + PX4_INFO("chunked load: loaded %u mixers", mixer_group.count()); if (mixer_group.count() != 8) { return 1; @@ -184,7 +189,6 @@ int test_mixer(int argc, char *argv[]) const int jmax = 5; pwm_limit_init(&pwm_limit); - should_arm = true; /* run through arming phase */ for (unsigned i = 0; i < output_max; i++) { @@ -194,50 +198,83 @@ int test_mixer(int argc, char *argv[]) r_page_servo_control_max[i] = PWM_DEFAULT_MAX; } - warnx("ARMING TEST: STARTING RAMP"); + PX4_INFO("PRE-ARM TEST: DISABLING SAFETY"); + /* mix */ + should_prearm = true; + mixed = mixer_group.mix(&outputs[0], output_max, NULL); + + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (unsigned i = 0; i < mixed; i++) { + + warnx("pre-arm:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + + if (i != actuator_controls_s::INDEX_THROTTLE) { + if (r_page_servos[i] < r_page_servo_control_min[i]) { + warnx("active servo < min"); + return 1; + } + + } else { + if (r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("throttle output != 0 (this check assumed the IO pass mixer!)"); + return 1; + } + } + } + + should_arm = true; + should_prearm = false; + + /* simulate another orb_copy() from actuator controls */ + for (unsigned i = 0; i < output_max; i++) { + actuator_controls[i] = 0.1f; + } + + PX4_INFO("ARMING TEST: STARTING RAMP"); unsigned sleep_quantum_us = 10000; hrt_abstime starttime = hrt_absolute_time(); unsigned sleepcount = 0; - while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { + while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) { /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { + + warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && r_page_servos[i] != r_page_servo_disarmed[i]) { - warnx("disarmed servo value mismatch"); + PX4_ERR("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]); return 1; } if (hrt_elapsed_time(&starttime) >= INIT_TIME_US && r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) { - warnx("ramp servo value mismatch"); + PX4_ERR("ramp servo value mismatch"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); sleepcount++; if (sleepcount % 10 == 0) { - printf("."); fflush(stdout); } } - printf("\n"); - - warnx("ARMING TEST: NORMAL OPERATION"); + PX4_INFO("ARMING TEST: NORMAL OPERATION"); for (int j = -jmax; j <= jmax; j++) { @@ -251,23 +288,24 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - warnx("mixed %d outputs (max %d)", mixed, output_max); + PX4_INFO("mixed %d outputs (max %d)", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; - if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + if (abs(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); - warnx("mixer violated predicted value"); + PX4_ERR("mixer violated predicted value"); return 1; } } } - warnx("ARMING TEST: DISARMING"); + PX4_INFO("ARMING TEST: DISARMING"); starttime = hrt_absolute_time(); sleepcount = 0; @@ -278,18 +316,20 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { + + warnx("disarmed:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + /* check mixed outputs to be zero during init phase */ if (r_page_servos[i] != r_page_servo_disarmed[i]) { - warnx("disarmed servo value mismatch"); + PX4_ERR("disarmed servo value mismatch"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -303,7 +343,7 @@ int test_mixer(int argc, char *argv[]) printf("\n"); - warnx("ARMING TEST: REARMING: STARTING RAMP"); + PX4_INFO("ARMING TEST: REARMING: STARTING RAMP"); starttime = hrt_absolute_time(); sleepcount = 0; @@ -314,7 +354,8 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); @@ -324,22 +365,22 @@ int test_mixer(int argc, char *argv[]) /* check ramp */ + warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || r_page_servos[i] > servo_predicted[i])) { - warnx("ramp servo value mismatch"); + PX4_ERR("ramp servo value mismatch"); return 1; } /* check post ramp phase */ if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && - fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + abs(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); - warnx("mixer violated predicted value"); + PX4_ERR("mixer violated predicted value"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -366,18 +407,18 @@ int test_mixer(int argc, char *argv[]) load_mixer_file(filename, &buf[0], sizeof(buf)); loaded = strlen(buf); - warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + PX4_INFO("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); unsigned mc_loaded = loaded; mixer_group.load_from_buf(&buf[0], mc_loaded); - warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count()); if (mixer_group.count() != 5) { - warnx("FAIL: Quad W mixer load failed"); + PX4_ERR("FAIL: Quad W mixer load failed"); return 1; } - warnx("SUCCESS: No errors in mixer test"); + PX4_INFO("SUCCESS: No errors in mixer test"); return 0; } @@ -397,5 +438,10 @@ mixer_callback(uintptr_t handle, control = actuator_controls[control_index]; + if (should_prearm && control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + control = NAN_VALUE; + } + return 0; } diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 107d616b6c21..f92c5f6316c5 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -69,15 +69,15 @@ test_mount(int argc, char *argv[]) /* check if microSD card is mounted */ struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { - warnx("no microSD card mounted, aborting file test"); + if (stat(PX4_ROOTFSDIR "/fs/microsd/", &buffer)) { + PX4_ERR("no microSD card mounted, aborting file test"); return 1; } /* list directory */ DIR *d; struct dirent *dir; - d = opendir("/fs/microsd"); + d = opendir(PX4_ROOTFSDIR "/fs/microsd"); if (d) { @@ -87,11 +87,11 @@ test_mount(int argc, char *argv[]) closedir(d); - warnx("directory listing ok (FS mounted and readable)"); + PX4_INFO("directory listing ok (FS mounted and readable)"); } else { /* failed opening dir */ - warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); + PX4_ERR("FAILED LISTING MICROSD ROOT DIRECTORY"); if (stat(cmd_filename, &buffer) == OK) { (void)unlink(cmd_filename); @@ -131,8 +131,8 @@ test_mount(int argc, char *argv[]) it_left_abort = abort_tries; } - warnx("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort, - fsync_tries, abort_tries, buf); + PX4_INFO("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort, + fsync_tries, abort_tries, buf); int it_left_fsync_prev = it_left_fsync; @@ -147,7 +147,7 @@ test_mount(int argc, char *argv[]) /* announce mode switch */ if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) { - warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC"); + PX4_INFO("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC"); fsync(fileno(stdout)); fsync(fileno(stderr)); usleep(20000); @@ -163,9 +163,9 @@ test_mount(int argc, char *argv[]) } else { /* this must be the first iteration, do something */ - cmd_fd = open(cmd_filename, O_TRUNC | O_WRONLY | O_CREAT); + cmd_fd = open(cmd_filename, O_TRUNC | O_WRONLY | O_CREAT, PX4_O_MODE_666); - warnx("First iteration of file test\n"); + PX4_INFO("First iteration of file test\n"); } char buf[64]; @@ -200,17 +200,17 @@ test_mount(int argc, char *argv[]) uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + int fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf + a, chunk_sizes[c]); if (wret != (int)chunk_sizes[c]) { - warn("WRITE ERROR!"); + PX4_ERR("WRITE ERROR!"); if ((0x3 & (uintptr_t)(write_buf + a))) { - warnx("memory is unaligned, align shift: %d", a); + PX4_ERR("memory is unaligned, align shift: %d", a); } return 1; @@ -237,14 +237,14 @@ test_mount(int argc, char *argv[]) usleep(200000); close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); + fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY); /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { int rret = read(fd, read_buf, chunk_sizes[c]); if (rret != (int)chunk_sizes[c]) { - warnx("READ ERROR!"); + PX4_ERR("READ ERROR!"); return 1; } @@ -253,24 +253,24 @@ test_mount(int argc, char *argv[]) for (unsigned j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j + a]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + PX4_WARN("COMPARISON ERROR: byte %d, align shift: %d", j, a); compare_ok = false; break; } } if (!compare_ok) { - warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + PX4_ERR("ABORTING FURTHER COMPARISON DUE TO ERROR"); return 1; } } - int ret = unlink("/fs/microsd/testfile"); + int ret = unlink(PX4_ROOTFSDIR "/fs/microsd/testfile"); close(fd); if (ret) { - warnx("UNLINKING FILE FAILED"); + PX4_ERR("UNLINKING FILE FAILED"); return 1; } @@ -284,7 +284,7 @@ test_mount(int argc, char *argv[]) /* we always reboot for the next test if we get here */ - warnx("Iteration done, rebooting.."); + PX4_INFO("Iteration done, rebooting.."); fsync(fileno(stdout)); fsync(fileno(stderr)); usleep(50000); diff --git a/src/systemcmds/tests/test_param.c b/src/systemcmds/tests/test_param.c deleted file mode 100644 index ab41841905b3..000000000000 --- a/src/systemcmds/tests/test_param.c +++ /dev/null @@ -1,97 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * 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 test_param.c - * - * Tests related to the parameter system. - */ - -#include -#include "systemlib/err.h" -#include "systemlib/param/param.h" -#include "tests.h" - -#define PARAM_MAGIC1 0x12345678 -#define PARAM_MAGIC2 0xa5a5a5a5 -PARAM_DEFINE_INT32(test, PARAM_MAGIC1); - -int -test_param(int argc, char *argv[]) -{ - param_t p; - - p = param_find("test"); - - if (p == PARAM_INVALID) { - errx(1, "test parameter not found"); - } - - if (param_reset(p) != OK) { - errx(1, "failed param reset"); - } - - param_type_t t = param_type(p); - - if (t != PARAM_TYPE_INT32) { - errx(1, "test parameter type mismatch (got %u)", (unsigned)t); - } - - int32_t val; - - if (param_get(p, &val) != OK) { - errx(1, "failed to read test parameter"); - } - - if (val != PARAM_MAGIC1) { - errx(1, "parameter value mismatch"); - } - - val = PARAM_MAGIC2; - - if (param_set(p, &val) != OK) { - errx(1, "failed to write test parameter"); - } - - if (param_get(p, &val) != OK) { - errx(1, "failed to re-read test parameter"); - } - - if ((uint32_t)val != PARAM_MAGIC2) { - errx(1, "parameter value mismatch after write"); - } - - warnx("parameter test PASS"); - - return 0; -} diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index 7f1323f2b926..a889488eabe2 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -46,7 +46,6 @@ #include #include #include -#include #include #include @@ -162,7 +161,7 @@ int test_ppm_loopback(int argc, char *argv[]) /* go and check values */ for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { - if (fabsf(rc_input.values[i] - pwm_values[i]) > 10) { + if (abs(rc_input.values[i] - pwm_values[i]) > 10) { warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]); (void)close(servo_fd); return ERROR; diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index fed11c8cb159..1f150816d479 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -47,7 +47,6 @@ #include #include #include -#include #include #include @@ -76,8 +75,8 @@ int test_rc(int argc, char *argv[]) bool rc_updated; orb_check(_rc_sub, &rc_updated); - warnx("Reading PPM values - press any key to abort"); - warnx("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes."); + PX4_INFO("Reading PPM values - press any key to abort"); + PX4_INFO("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes."); if (rc_updated) { @@ -107,8 +106,8 @@ int test_rc(int argc, char *argv[]) /* go and check values */ for (unsigned i = 0; i < rc_input.channel_count; i++) { - if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) { - warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]); + if (abs(rc_input.values[i] - rc_last.values[i]) > 20) { + PX4_ERR("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]); (void)close(_rc_sub); return ERROR; } @@ -117,13 +116,13 @@ int test_rc(int argc, char *argv[]) } if (rc_last.channel_count != rc_input.channel_count) { - warnx("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count); + PX4_ERR("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count); (void)close(_rc_sub); return ERROR; } if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) { - warnx("TIMEOUT, less than 10 Hz updates"); + PX4_ERR("TIMEOUT, less than 10 Hz updates"); (void)close(_rc_sub); return ERROR; } @@ -137,11 +136,11 @@ int test_rc(int argc, char *argv[]) } } else { - warnx("failed reading RC input data"); + PX4_ERR("failed reading RC input data"); return ERROR; } - warnx("PPM CONTINUITY TEST PASSED SUCCESSFULLY!"); + PX4_INFO("PPM CONTINUITY TEST PASSED SUCCESSFULLY!"); return 0; } diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index 8d27247f79ee..d663a7c3f0c6 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -39,6 +39,7 @@ */ #include +#include #include @@ -47,13 +48,12 @@ #include #include #include -#include #include #include #include -#include +//#include #include "tests.h" @@ -95,7 +95,7 @@ accel(int argc, char *argv[], const char *path) struct accel_report buf; int ret; - fd = open(path, O_RDONLY); + fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tACCEL: open fail, run or or first.\n"); @@ -106,7 +106,7 @@ accel(int argc, char *argv[], const char *path) usleep(100000); /* read data - expect samples */ - ret = read(fd, &buf, sizeof(buf)); + ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tACCEL: read1 fail (%d)\n", ret); @@ -130,7 +130,7 @@ accel(int argc, char *argv[], const char *path) /* Let user know everything is ok */ printf("\tOK: ACCEL passed all tests successfully\n"); - close(fd); + px4_close(fd); return OK; } @@ -145,7 +145,7 @@ gyro(int argc, char *argv[], const char *path) struct gyro_report buf; int ret; - fd = open(path, O_RDONLY); + fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tGYRO: open fail, run or first.\n"); @@ -156,7 +156,7 @@ gyro(int argc, char *argv[], const char *path) usleep(5000); /* read data - expect samples */ - ret = read(fd, &buf, sizeof(buf)); + ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tGYRO: read fail (%d)\n", ret); @@ -175,7 +175,7 @@ gyro(int argc, char *argv[], const char *path) /* Let user know everything is ok */ printf("\tOK: GYRO passed all tests successfully\n"); - close(fd); + px4_close(fd); return OK; } @@ -190,7 +190,7 @@ mag(int argc, char *argv[], const char *path) struct mag_report buf; int ret; - fd = open(path, O_RDONLY); + fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tMAG: open fail, run or first.\n"); @@ -201,7 +201,7 @@ mag(int argc, char *argv[], const char *path) usleep(5000); /* read data - expect samples */ - ret = read(fd, &buf, sizeof(buf)); + ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tMAG: read fail (%d)\n", ret); @@ -220,7 +220,7 @@ mag(int argc, char *argv[], const char *path) /* Let user know everything is ok */ printf("\tOK: MAG passed all tests successfully\n"); - close(fd); + px4_close(fd); return OK; } @@ -235,7 +235,7 @@ baro(int argc, char *argv[], const char *path) struct baro_report buf; int ret; - fd = open(path, O_RDONLY); + fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tBARO: open fail, run or first.\n"); @@ -246,7 +246,7 @@ baro(int argc, char *argv[], const char *path) usleep(5000); /* read data - expect samples */ - ret = read(fd, &buf, sizeof(buf)); + ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tBARO: read fail (%d)\n", ret); @@ -259,7 +259,7 @@ baro(int argc, char *argv[], const char *path) /* Let user know everything is ok */ printf("\tOK: BARO passed all tests successfully\n"); - close(fd); + px4_close(fd); return OK; } diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index 7dea9ac54987..a862d41d1a9c 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -46,14 +46,13 @@ #include #include #include -#include #include #include #include #include -#include +//#include #include "tests.h" diff --git a/src/systemcmds/tests/test_sleep.c b/src/systemcmds/tests/test_sleep.c index e21763f43fa6..09f2bd2dcafa 100644 --- a/src/systemcmds/tests/test_sleep.c +++ b/src/systemcmds/tests/test_sleep.c @@ -37,6 +37,7 @@ ****************************************************************************/ #include +#include #include @@ -45,7 +46,6 @@ #include #include #include -#include #include diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c index 48e084326ba3..d89f90c683aa 100644 --- a/src/systemcmds/tests/test_time.c +++ b/src/systemcmds/tests/test_time.c @@ -45,7 +45,6 @@ #include #include #include -#include #include @@ -104,7 +103,7 @@ cycletime(void) ****************************************************************************/ /**************************************************************************** - * Name: test_led + * Name: test_time ****************************************************************************/ int test_time(int argc, char *argv[]) @@ -153,11 +152,11 @@ int test_time(int argc, char *argv[]) } if (deltadelta > 1000) { - fprintf(stderr, "h %llu c %llu d %lld\n", h, c, delta - lowdelta); + fprintf(stderr, "h %" PRIu64 " c %" PRIu64 " d %" PRId64 "\n", h, c, delta - lowdelta); } } - printf("Maximum jitter %lldus\n", maxdelta); + printf("Maximum jitter %" PRId64 "us\n", maxdelta); return 0; } diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c index 5ee2b6c5b53f..7c138efca112 100644 --- a/src/systemcmds/tests/test_uart_baudchange.c +++ b/src/systemcmds/tests/test_uart_baudchange.c @@ -38,6 +38,7 @@ ****************************************************************************/ #include +#include #include @@ -46,7 +47,6 @@ #include #include #include -#include #include #include @@ -109,12 +109,15 @@ int test_uart_baudchange(int argc, char *argv[]) int termios_state = 0; + int ret; + #define UART_BAUDRATE_RUNTIME_CONF #ifdef UART_BAUDRATE_RUNTIME_CONF if ((termios_state = tcgetattr(uart2, &uart2_config)) < 0) { printf("ERROR getting termios config for UART2: %d\n", termios_state); - exit(termios_state); + ret = termios_state; + goto cleanup; } memcpy(&uart2_config_original, &uart2_config, sizeof(struct termios)); @@ -122,18 +125,21 @@ int test_uart_baudchange(int argc, char *argv[]) /* Set baud rate */ if (cfsetispeed(&uart2_config, B9600) < 0 || cfsetospeed(&uart2_config, B9600) < 0) { printf("ERROR setting termios config for UART2: %d\n", termios_state); - exit(ERROR); + ret = ERROR; + goto cleanup; } if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config)) < 0) { printf("ERROR setting termios config for UART2\n"); - exit(termios_state); + ret = termios_state; + goto cleanup; } /* Set back to original settings */ if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config_original)) < 0) { printf("ERROR setting termios config for UART2\n"); - exit(termios_state); + ret = termios_state; + goto cleanup; } #endif @@ -156,4 +162,8 @@ int test_uart_baudchange(int argc, char *argv[]) printf("uart2_nwrite %d\n", uart2_nwrite); return OK; +cleanup: + close(uart2); + return ret; + } diff --git a/src/systemcmds/tests/test_uart_console.c b/src/systemcmds/tests/test_uart_console.c index e49fc1f94cff..ad1b2776e176 100644 --- a/src/systemcmds/tests/test_uart_console.c +++ b/src/systemcmds/tests/test_uart_console.c @@ -46,7 +46,6 @@ #include #include #include -#include #include diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c index a929a6f94d8c..a45254bf9369 100644 --- a/src/systemcmds/tests/test_uart_loopback.c +++ b/src/systemcmds/tests/test_uart_loopback.c @@ -43,10 +43,10 @@ #include #include +#include #include #include #include -#include #include diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c index ce63071ac28d..bc4a0bddad96 100644 --- a/src/systemcmds/tests/test_uart_send.c +++ b/src/systemcmds/tests/test_uart_send.c @@ -46,7 +46,6 @@ #include #include #include -#include #include diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 546f3ceb577b..e16e0adb5690 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -48,11 +48,10 @@ #include #include #include -#include #include -#include +//#include #include @@ -97,7 +96,9 @@ const struct { {"hott_telemetry", test_hott_telemetry, OPT_NOJIGTEST | OPT_NOALLTEST}, {"tone", test_tone, 0}, {"sleep", test_sleep, OPT_NOJIGTEST}, +#ifdef __PX4_NUTTX {"time", test_time, OPT_NOJIGTEST}, +#endif {"perf", test_perf, OPT_NOJIGTEST}, {"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST}, {"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST}, diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index ca48a86a82a3..fe9614bc6976 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012, 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 @@ -36,8 +35,8 @@ * @file top.c * Tool similar to UNIX top command * @see http://en.wikipedia.org/wiki/Top_unix - * - * @author Lorenz Meier + * + * @author Lorenz Meier */ #include @@ -49,208 +48,34 @@ #include #include +#include #include -#define CL "\033[K" // clear line - /** * Start the top application. */ -__EXPORT int top_main(void); - -extern struct system_load_s system_load; - -static const char * -tstate_name(const tstate_t s) -{ - switch (s) { - case TSTATE_TASK_INVALID: return "init"; - - case TSTATE_TASK_PENDING: return "PEND"; - case TSTATE_TASK_READYTORUN: return "READY"; - case TSTATE_TASK_RUNNING: return "RUN"; - - case TSTATE_TASK_INACTIVE: return "inact"; - case TSTATE_WAIT_SEM: return "w:sem"; -#ifndef CONFIG_DISABLE_SIGNALS - case TSTATE_WAIT_SIG: return "w:sig"; -#endif -#ifndef CONFIG_DISABLE_MQUEUE - case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe"; - case TSTATE_WAIT_MQNOTFULL: return "w:mqf"; -#endif -#ifdef CONFIG_PAGING - case TSTATE_WAIT_PAGEFILL: return "w:pgf"; -#endif - - default: - return "ERROR"; - } -} +__EXPORT int top_main(int argc, char *argv[]); int -top_main(void) +top_main(int argc, char *argv[]) { - uint64_t total_user_time = 0; - - int running_count = 0; - int blocked_count = 0; - - uint64_t new_time = hrt_absolute_time(); - uint64_t interval_start_time = new_time; + hrt_abstime curr_time = hrt_absolute_time(); - uint64_t last_times[CONFIG_MAX_TASKS]; - float curr_loads[CONFIG_MAX_TASKS]; - - for (int t = 0; t < CONFIG_MAX_TASKS; t++) - last_times[t] = 0; - - float interval_time_ms_inv = 0.f; + struct print_load_s load; + init_print_load_s(curr_time, &load); /* clear screen */ - printf("\033[2J"); - - for (;;) { - int i; - uint64_t curr_time_us; - uint64_t idle_time_us; - - curr_time_us = hrt_absolute_time(); - idle_time_us = system_load.tasks[0].total_runtime; - - if (new_time > interval_start_time) - interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000)); + dprintf(1, "\033[2J\n"); - running_count = 0; - blocked_count = 0; - total_user_time = 0; - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - uint64_t interval_runtime; - - if (system_load.tasks[i].valid) { - switch (system_load.tasks[i].tcb->task_state) { - case TSTATE_TASK_PENDING: - case TSTATE_TASK_READYTORUN: - case TSTATE_TASK_RUNNING: - running_count++; - break; - - case TSTATE_TASK_INVALID: - case TSTATE_TASK_INACTIVE: - case TSTATE_WAIT_SEM: -#ifndef CONFIG_DISABLE_SIGNALS - case TSTATE_WAIT_SIG: -#endif -#ifndef CONFIG_DISABLE_MQUEUE - case TSTATE_WAIT_MQNOTEMPTY: - case TSTATE_WAIT_MQNOTFULL: -#endif -#ifdef CONFIG_PAGING - case TSTATE_WAIT_PAGEFILL: -#endif - blocked_count++; - break; - } - } - - interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && - system_load.tasks[i].total_runtime > last_times[i]) - ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 - : 0; - - last_times[i] = system_load.tasks[i].total_runtime; - - if (system_load.tasks[i].valid && (new_time > interval_start_time)) { - curr_loads[i] = interval_runtime * interval_time_ms_inv; - - if (i > 0) - total_user_time += interval_runtime; - } else - curr_loads[i] = 0; - } - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - if (system_load.tasks[i].valid && (new_time > interval_start_time)) { - if (system_load.tasks[i].tcb->pid == 0) { - float idle; - float task_load; - float sched_load; - - idle = curr_loads[0]; - task_load = (float)(total_user_time) * interval_time_ms_inv; - - /* this can happen if one tasks total runtime was not computed - correctly by the scheduler instrumentation TODO */ - if (task_load > (1.f - idle)) - task_load = (1.f - idle); - - sched_load = 1.f - idle - task_load; - - /* print system information */ - printf("\033[H"); /* move cursor home and clear screen */ - printf(CL "Processes: %d total, %d running, %d sleeping\n", - system_load.total_count, - running_count, - blocked_count); - printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n", - (double)(task_load * 100.f), - (double)(sched_load * 100.f), - (double)(idle * 100.f)); - printf(CL "Uptime: %.3fs total, %.3fs idle\n\n", - (double)curr_time_us / 1000000.d, - (double)idle_time_us / 1000000.d); - - /* header for task list */ - printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n", - "PID", - CONFIG_TASK_NAME_SIZE, "COMMAND", - "CPU(ms)", - "CPU(%)", - "USED/STACK", - "PRIO(BASE)", -#if CONFIG_RR_INTERVAL > 0 - "TSLICE" -#else - "STATE" -#endif - ); - } - - unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; - unsigned stack_free = 0; - uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; - - while (stack_free < stack_size) { - if (*stack_sweeper++ != 0xff) - break; - - stack_free++; - } - - printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", - system_load.tasks[i].tcb->pid, - CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, - (system_load.tasks[i].total_runtime / 1000), - (int)(curr_loads[i] * 100.0f), - (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), - stack_size - stack_free, - stack_size, - system_load.tasks[i].tcb->sched_priority, - system_load.tasks[i].tcb->base_priority); - -#if CONFIG_RR_INTERVAL > 0 - /* print scheduling info with RR time slice */ - printf(" %6d\n", system_load.tasks[i].tcb->timeslice); -#else - // print task state instead - printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state)); -#endif - } - } + if (argc > 1 && !strcmp(argv[1], "once")) { + print_load(curr_time, 1, &load); + sleep(1); + print_load(hrt_absolute_time(), 1, &load); + return 0; + } - interval_start_time = new_time; + for (;;) { + print_load(curr_time, 1, &load); /* Sleep 200 ms waiting for user input five times ~ 1s */ for (int k = 0; k < 5; k++) { @@ -279,7 +104,7 @@ top_main(void) usleep(200000); } - new_time = hrt_absolute_time(); + curr_time = hrt_absolute_time(); } return OK; diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index a248dfb79c28..28cd9886278e 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -41,6 +41,7 @@ */ #include +#include #include #include #include @@ -57,6 +58,9 @@ static const char sz_ver_all_str[] = "all"; static const char mcu_ver_str[] = "mcu"; static const char mcu_uid_str[] = "uid"; +const char *px4_git_version = PX4_GIT_VERSION_STR; +const uint64_t px4_git_version_binary = PX4_GIT_VERSION_BINARY; + static void usage(const char *reason) { if (reason != NULL) { @@ -85,10 +89,12 @@ int ver_main(int argc, char *argv[]) if (ret == 0) { printf("ver hwcmp match: %s\n", HW_ARCH); } + return ret; } else { - errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); + warn("Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); + return 1; } } @@ -122,7 +128,7 @@ int ver_main(int argc, char *argv[]) if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { char rev; - char* revstr; + char *revstr; int chip_version = mcu_version(&rev, &revstr); @@ -134,9 +140,9 @@ int ver_main(int argc, char *argv[]) if (chip_version < MCU_REV_STM32F4_REV_3) { 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\n", rev); + "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\n", rev); } } @@ -148,14 +154,15 @@ int ver_main(int argc, char *argv[]) mcu_unique_id(uid); - printf("UID: %X:%X:%X \n",uid[0],uid[1],uid[2]); + printf("UID: %X:%X:%X \n", uid[0], uid[1], uid[2]); ret = 0; } if (ret == 1) { - errx(1, "unknown command.\n"); + warn("unknown command.\n"); + return 1; } } else { From 417e52d8084e8bf0c0567d5573f36f925589059f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:10:45 +1100 Subject: [PATCH 245/293] unittests: update from upstream --- unittests/CMakeLists.txt | 100 +++++++++++++++++++++++++++++++++++++-- unittests/debug.h | 4 ++ unittests/param_test.cpp | 4 +- unittests/sbus2_test.cpp | 12 ++--- unittests/st24_test.cpp | 5 +- unittests/sumd_test.cpp | 5 +- 6 files changed, 111 insertions(+), 19 deletions(-) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index ae87cfca7aa7..4285520b0c9c 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -1,18 +1,41 @@ cmake_minimum_required(VERSION 2.8) + +include( CMakeForceCompiler ) +#set( CMAKE_SYSTEM_NAME px4_posix_clang ) +CMAKE_FORCE_C_COMPILER( clang Clang ) +CMAKE_FORCE_CXX_COMPILER( clang++ Clang ) +#set( CMAKE_C_COMPILER /opt/clang-3.4.2/bin/clang ) +#set( CMAKE_CXX_COMPILER /opt/clang-3.4.2/bin/clang++ ) +#set( CMAKE_FIND_ROOT_PATH /opt/clang-3.4.2/ ) +#set( CMAKE_FIND_ROOT_PATH_MODE_PROGRAM_NEVER ) +#set( CMAKE_FIND_ROOT_PATH_MODE_LIBARARY_ONLY ) +#set( CMAKE_FIND_ROOT_PATH_MODE_INCLUDE_ONLY ) + +if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang") + add_compile_options(-Qunused-arguments) +endif() +if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + add_compile_options(-Qunused-arguments) +endif() + project(unittests) enable_testing() + include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g -Wall") elseif(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g -Wall") else() message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") endif() -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g") + +#set(CMAKE_INCLUDE_SYSTEM_FLAG_C "-isystem" ) +#set(CMAKE_INCLUDE_SYSTEM_FLAG_CXX "-isystem" ) set(GTEST_DIR gtest) add_subdirectory(${GTEST_DIR}) @@ -22,9 +45,14 @@ set(PX_SRC ${CMAKE_SOURCE_DIR}/../src) include_directories(${CMAKE_SOURCE_DIR}) include_directories(${PX_SRC}) include_directories(${PX_SRC}/modules) +include_directories(${PX_SRC}/modules/uORB) include_directories(${PX_SRC}/lib) include_directories(${PX_SRC}/drivers) include_directories(${PX_SRC}/platforms) +include_directories(${PX_SRC}/platforms/posix/include) +include_directories(${PX_SRC}/platforms/posix/px4_layer) +include_directories(${PX_SRC}/drivers/device ) + add_definitions(-D__EXPORT=) add_definitions(-D__PX4_TESTS) @@ -34,19 +62,57 @@ add_definitions(-DERROR=-1) add_definitions(-DOK=0) add_definitions(-D_UNIT_TEST=) add_definitions(-D__PX4_POSIX) -add_definitions(-D__PX4_LINUX) # check add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) function(add_gtest) foreach(test_name ${ARGN}) - target_link_libraries(${test_name} gtest_main) + if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + target_link_libraries(${test_name} gtest_main pthread ) + add_definitions(-D__PX4_DARWIN) + else() + target_link_libraries(${test_name} gtest_main pthread rt ) + add_definitions(-D__PX4_LINUX) + endif() add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) add_dependencies(unittests ${test_name}) endforeach() endfunction() +add_library( px4_platform +# ${PX_SRC}/platforms/common/px4_getopt.c + ${PX_SRC}/platforms/posix/px4_layer/px4_log.c + ${PX_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp + ${PX_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp + ${PX_SRC}/platforms/posix/work_queue/work_lock.c + ${PX_SRC}/platforms/posix/work_queue/hrt_queue.c + ${PX_SRC}/platforms/posix/work_queue/work_queue.c + ${PX_SRC}/platforms/posix/work_queue/queue.c + ${PX_SRC}/platforms/posix/work_queue/work_cancel.c + ${PX_SRC}/platforms/posix/work_queue/hrt_work_cancel.c + ${PX_SRC}/platforms/posix/work_queue/hrt_thread.c + ${PX_SRC}/platforms/posix/work_queue/work_thread.c + ${PX_SRC}/platforms/posix/work_queue/dq_rem.c + ${PX_SRC}/platforms/posix/work_queue/sq_addlast.c + ${PX_SRC}/platforms/posix/work_queue/sq_addafter.c + ${PX_SRC}/platforms/posix/work_queue/dq_remfirst.c + ${PX_SRC}/platforms/posix/work_queue/sq_remfirst.c + ${PX_SRC}/platforms/posix/work_queue/dq_addlast.c + ${PX_SRC}/platforms/posix/px4_layer/lib_crc32.c + ${PX_SRC}/platforms/posix/px4_layer/drv_hrt.c + ${PX_SRC}/drivers/device/device_posix.cpp + ${PX_SRC}/drivers/device/vdev.cpp + ${PX_SRC}/drivers/device/vfile.cpp + ${PX_SRC}/drivers/device/vdev_posix.cpp + ${PX_SRC}/drivers/device/i2c_posix.cpp + ${PX_SRC}/drivers/device/sim.cpp + ${PX_SRC}/drivers/device/ringbuffer.cpp + ) + +#target_include_directories( px4_platform PUBLIC ${PX_SRC}/platforms ) + + # add each test add_executable(autodeclination_test autodeclination_test.cpp ${PX_SRC}/lib/geo_lookup/geo_mag_declination.c) @@ -64,26 +130,34 @@ add_executable(mixer_test mixer_test.cpp hrt.cpp ${PX_SRC}/modules/systemlib/mixer/mixer_simple.cpp ${PX_SRC}/modules/systemlib/pwm_limit/pwm_limit.c ${PX_SRC}/systemcmds/tests/test_mixer.cpp) +target_link_libraries( mixer_test px4_platform ) + + add_gtest(mixer_test) # conversion_test add_executable(conversion_test conversion_test.cpp ${PX_SRC}/systemcmds/tests/test_conv.cpp) +target_link_libraries( conversion_test px4_platform ) add_gtest(conversion_test) # sbus2_test add_executable(sbus2_test sbus2_test.cpp hrt.cpp) +target_link_libraries( sbus2_test px4_platform ) add_gtest(sbus2_test) # st24_test add_executable(st24_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c) +target_link_libraries( st24_test px4_platform ) add_gtest(st24_test) # sumd_test add_executable(sumd_test sumd_test.cpp hrt.cpp ${PX_SRC}/lib/rc/sumd.c) +target_link_libraries( sumd_test px4_platform ) add_gtest(sumd_test) # sf0x_test add_executable(sf0x_test sf0x_test.cpp ${PX_SRC}/drivers/sf0x/sf0x_parser.cpp) +target_link_libraries( sf0x_test px4_platform ) add_gtest(sf0x_test) # param_test @@ -93,4 +167,20 @@ add_executable(param_test param_test.cpp ${PX_SRC}/modules/systemlib/param/param.c ${PX_SRC}/modules/systemlib/bson/tinybson.c ) +target_link_libraries( param_test px4_platform ) + add_gtest(param_test) + +# uorb test +add_executable(uorb_tests uorb_unittests/uORBCommunicator_gtests.cpp + uorb_unittests/uORBCommunicatorMock.cpp + uorb_unittests/uORBCommunicatorMockLoopback.cpp + ${PX_SRC}/modules/uORB/uORBDevices_posix.cpp + ${PX_SRC}/modules/uORB/uORBManager_posix.cpp + ${PX_SRC}/modules/uORB/objects_common.cpp + ${PX_SRC}/modules/uORB/uORBUtils.cpp + ${PX_SRC}/modules/uORB/uORB.cpp + ) +target_link_libraries( uorb_tests px4_platform ) + +add_gtest(uorb_tests) diff --git a/unittests/debug.h b/unittests/debug.h index ac321312f5a6..4071703da2c0 100644 --- a/unittests/debug.h +++ b/unittests/debug.h @@ -4,3 +4,7 @@ #include #define lowsyslog warnx #define dbg warnx + +#if !defined(ASSERT) +# define ASSERT(x) assert((x)) +#endif diff --git a/unittests/param_test.cpp b/unittests/param_test.cpp index 44ae4df06803..bda49ae86d08 100644 --- a/unittests/param_test.cpp +++ b/unittests/param_test.cpp @@ -52,8 +52,8 @@ void _assert_parameter_int_value(param_t param, int32_t expected) { int32_t value; int result = param_get(param, &value); - ASSERT_EQ(0, result) << printf("param_get (%i) did not return parameter\n", param); - ASSERT_EQ(expected, value) << printf("value for param (%i) doesn't match default value\n", param); + ASSERT_EQ(0, result) << printf("param_get (%lu) did not return parameter\n", param); + ASSERT_EQ(expected, value) << printf("value for param (%lu) doesn't match default value\n", param); } void _set_all_int_parameters_to(int32_t value) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 41f49627d4c0..54c7fc412b85 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -33,11 +33,11 @@ TEST(SBUS2Test, SBUS2) // Init the parser uint8_t frame[30]; unsigned partial_frame_count = 0; - uint16_t rc_values[18]; - uint16_t num_values; - bool sbus_failsafe; - bool sbus_frame_drop; - uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + //uint16_t rc_values[18]; + //uint16_t num_values; + //bool sbus_failsafe; + //bool sbus_frame_drop; + //uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); float last_time = 0; @@ -59,7 +59,7 @@ TEST(SBUS2Test, SBUS2) last_time = f; // Pipe the data into the parser - hrt_abstime now = hrt_absolute_time(); + //hrt_abstime now = hrt_absolute_time(); //if (partial_frame_count % 25 == 0) //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp index 42fa07ae7439..c7fded68d437 100644 --- a/unittests/st24_test.cpp +++ b/unittests/st24_test.cpp @@ -41,7 +41,7 @@ TEST(ST24Test, ST24) last_time = f; // Pipe the data into the parser - hrt_abstime now = hrt_absolute_time(); + //hrt_abstime now = hrt_absolute_time(); uint8_t rssi; uint8_t rx_count; @@ -53,8 +53,7 @@ TEST(ST24Test, ST24) //warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); for (unsigned i = 0; i < channel_count; i++) { - - int16_t val = channels[i]; + //int16_t val = channels[i]; //warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } } diff --git a/unittests/sumd_test.cpp b/unittests/sumd_test.cpp index bf99709475b4..65c944676c6c 100644 --- a/unittests/sumd_test.cpp +++ b/unittests/sumd_test.cpp @@ -41,7 +41,7 @@ TEST(SUMDTest, SUMD) last_time = f; // Pipe the data into the parser - hrt_abstime now = hrt_absolute_time(); + //hrt_abstime now = hrt_absolute_time(); uint8_t rssi; uint8_t rx_count; @@ -53,8 +53,7 @@ TEST(SUMDTest, SUMD) //warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); for (unsigned i = 0; i < channel_count; i++) { - - int16_t val = channels[i]; + //int16_t val = channels[i]; //warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } } From 9cacdf3dc48aed471c16e09b040fd58c3b17a2ce Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:11:00 +1100 Subject: [PATCH 246/293] Tools: update from upstream --- Tools/check_code_style.sh | 57 +++++++-- Tools/fix_code_style.sh | 32 ++--- Tools/generate_listener.py | 162 ++++++++++++++++-------- Tools/posix_apps.py | 54 +++++--- Tools/px4params/srcparser.py | 117 ++++++++++------- Tools/px4params/srcscanner.py | 18 ++- Tools/px4params/xmlout.py | 6 +- Tools/px_generate_uorb_topic_headers.py | 67 ++++++++-- Tools/px_mkfw.py | 6 + Tools/px_process_params.py | 13 +- Tools/px_romfs_pruner.py | 14 +- Tools/qurt_apps.py | 16 +++ Tools/sdlog2/sdlog2_dump.py | 1 - 13 files changed, 392 insertions(+), 171 deletions(-) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index b8806f3164ee..c3dd2a3f3254 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -1,16 +1,55 @@ #!/usr/bin/env bash set -eu failed=0 -for fn in $(find . -path './src/lib/uavcan' -prune -o \ - -path './src/lib/mathlib/CMSIS' -prune -o \ - -path './src/lib/eigen' -prune -o \ - -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \ - -path './NuttX' -prune -o \ +for fn in $(find src/examples \ + src/systemcmds \ + src/include \ + src/drivers \ + src/platforms \ + src/firmware \ + src/lib/launchdetection \ + src/lib/geo \ + src/lib/geo_lookup \ + src/lib/conversion \ + src/lib/rc \ + src/lib/version \ + src/modules/attitude_estimator_q \ + src/modules/gpio_led \ + src/modules/land_detector \ + src/modules/muorb \ + src/modules/px4iofirmware \ + src/modules/param \ + src/modules/sensors \ + src/modules/simulator \ + src/modules/uORB \ + src/modules/bottle_drop \ + src/modules/dataman \ + src/modules/fixedwing_backside \ + src/modules/segway \ + src/modules/local_position_estimator \ + src/modules/unit_test \ + src/modules/systemlib \ + src/modules/controllib \ -path './Build' -prune -o \ -path './mavlink' -prune -o \ - -path './unittests/gtest' -prune -o \ + -path './NuttX' -prune -o \ + -path './src/lib/eigen' -prune -o \ + -path './src/lib/mathlib/CMSIS' -prune -o \ + -path './src/modules/uavcan/libuavcan' -prune -o \ + -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \ + -path './src/modules/ekf_att_pos_estimator' -prune -o \ + -path './src/modules/sdlog2' -prune -o \ + -path './src/modules/uORB' -prune -o \ + -path './src/modules/vtol_att_control' -prune -o \ -path './unittests/build' -prune -o \ - -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -type f); do + -path './unittests/gtest' -prune -o \ + -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' \ + -not -name '*generated*' \ + -not -name '*uthash.h' \ + -not -name '*utstring.h' \ + -not -name '*utlist.h' \ + -not -name '*utarray.h' \ + -type f); do if [ -f "$fn" ]; then ./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty @@ -18,7 +57,7 @@ for fn in $(find . -path './src/lib/uavcan' -prune -o \ rm -f $fn.pretty if [ $diffsize -ne 0 ]; then failed=1 - echo $diffsize $fn + echo $fn 'bad formatting, please run "./Tools/fix_code_style.sh' $fn'"' fi fi done @@ -27,6 +66,6 @@ if [ $failed -eq 0 ]; then echo "Format checks passed" exit 0 else - echo "Format checks failed; please run ./Tools/fix_code_style.sh on each file" + echo "Format checks failed" exit 1 fi diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh index e73a5a8af2cc..d8b424ae5670 100755 --- a/Tools/fix_code_style.sh +++ b/Tools/fix_code_style.sh @@ -1,22 +1,16 @@ -#!/bin/sh +#!/bin/bash + +ASTYLE_VER=`astyle --version` +ASTYLE_VER_REQUIRED="Artistic Style Version 2.05.1" + +if [ "$ASTYLE_VER" != "$ASTYLE_VER_REQUIRED" ]; then + echo "Error: you're using ${ASTYLE_VER}, but PX4 requires ${ASTYLE_VER_REQUIRED}" + echo "You can get the correct version here: https://github.com/PX4/astyle/releases/tag/2.05.1" + exit 1 +fi + +DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) astyle \ - --style=linux \ - --indent=force-tab=8 \ - --indent-cases \ - --indent-preprocessor \ - --break-blocks=all \ - --pad-oper \ - --pad-header \ - --unpad-paren \ - --keep-one-line-blocks \ - --keep-one-line-statements \ - --align-pointer=name \ - --align-reference=name \ - --suffix=none \ - --ignore-exclude-errors-x \ - --lineend=linux \ - --exclude=EASTL \ - --add-brackets \ - --max-code-length=120 \ + --options=$DIR/astylerc \ --preserve-date \ $* diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 5cfe4a628130..f097cb741b3d 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -1,6 +1,7 @@ #!/usr/bin/python import glob +import os import sys # This script is run from Build/_default.build/$(PX4_BASE)/Firmware/src/systemcmds/topic_listener @@ -16,30 +17,53 @@ temp_list_floats = [] temp_list_uint64 = [] temp_list_bool = [] - if("actuator_control" not in m and "pwm_input" not in m and "position_setpoint" not in m): + if("pwm_input" not in m and "position_setpoint" not in m): temp_list = [] f = open(m,'r') for line in f.readlines(): - if(line.split(' ')[0] == "float32"): + if ('float32[' in line.split(' ')[0]): + num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) + temp_list.append(("float_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) + elif ('float64[' in line.split(' ')[0]): + num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) + temp_list.append(("double_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) + elif ('uint64[' in line.split(' ')[0]): + num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) + temp_list.append(("uint64_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) + elif(line.split(' ')[0] == "float32"): temp_list.append(("float",line.split(' ')[1].split('\t')[0].split('\n')[0])) - elif(line.split(' ')[0] == "uint64"): + elif(line.split(' ')[0] == "float64"): + temp_list.append(("double",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "uint64") and len(line.split('=')) == 1: temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0])) - elif (line.split(' ')[0] == "bool"): + elif(line.split(' ')[0] == "uint32") and len(line.split('=')) == 1: + temp_list.append(("uint32",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "uint16") and len(line.split('=')) == 1: + temp_list.append(("uint16",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "int64") and len(line.split('=')) == 1: + temp_list.append(("int64",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "int32") and len(line.split('=')) == 1: + temp_list.append(("int32",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "int16") and len(line.split('=')) == 1: + temp_list.append(("int16",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif (line.split(' ')[0] == "bool") and len(line.split('=')) == 1: temp_list.append(("bool",line.split(' ')[1].split('\t')[0].split('\n')[0])) elif (line.split(' ')[0] == "uint8") and len(line.split('=')) == 1: temp_list.append(("uint8",line.split(' ')[1].split('\t')[0].split('\n')[0])) - elif ('float32[' in line.split(' ')[0]): - num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) - temp_list.append(("float_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) + elif (line.split(' ')[0] == "int8") and len(line.split('=')) == 1: + temp_list.append(("int8",line.split(' ')[1].split('\t')[0].split('\n')[0])) f.close() - messages.append(m.split('/')[-1].split('.')[0]) - message_elements.append(temp_list) + (m_head, m_tail) = os.path.split(m) + message = m_tail.split('.')[0] + if message != "actuator_controls": + messages.append(message) + message_elements.append(temp_list) + #messages.append(m.split('/')[-1].split('.')[0]) num_messages = len(messages); -print -print """ +print(""" /**************************************************************************** * @@ -75,30 +99,38 @@ ****************************************************************************/ /** - * @file topic_listener.cpp, autogenerated by Tools/generate_listener.py + * @file topic_listener.cpp + * + * Autogenerated by Tools/generate_listener.py * * Tool for listening to topics when running flight stack on linux. */ + #include #include #include -#include -#include -#include -#include -#include -#include #include #include #include +#include +#include #define __STDC_FORMAT_MACROS #include -""" + +#ifndef PRIu64 +#define PRIu64 "llu" +#endif + +#ifndef PRId64 +#define PRId64 "lld" +#endif + +""") for m in messages: - print "#include " % m + print("#include " % m) -print """ +print(""" extern "C" __EXPORT int listener_main(int argc, char *argv[]); int listener_main(int argc, char *argv[]) { @@ -108,45 +140,67 @@ printf("need at least two arguments: topic name, number of messages to print\\n"); return 1; } -""" -print "\tuint32_t num_msgs = (uint32_t)std::stoi(argv[2],NULL,10);" -print "\tif(strncmp(argv[1],\"%s\",50)== 0) {" % messages[0] -print "\t\tsub = orb_subscribe(ORB_ID(%s));" % messages[0] -print "\t\tID = ORB_ID(%s);" % messages[0] -print "\t\tstruct %s_s container;" % messages[0] -print "\t\tmemset(&container, 0, sizeof(container));" +""") +print("\tunsigned num_msgs = atoi(argv[2]);") +print("\tif (strncmp(argv[1],\"%s\",50) == 0) {" % messages[0]) +print("\t\tsub = orb_subscribe(ORB_ID(%s));" % messages[0]) +print("\t\tID = ORB_ID(%s);" % messages[0]) +print("\t\tstruct %s_s container;" % messages[0]) +print("\t\tmemset(&container, 0, sizeof(container));") for index,m in enumerate(messages[1:]): - print "\t} else if(strncmp(argv[1],\"%s\",50) == 0) {" % m - print "\t\tsub = orb_subscribe(ORB_ID(%s));" % m - print "\t\tID = ORB_ID(%s);" % m - print "\t\tstruct %s_s container;" % m - print "\t\tmemset(&container, 0, sizeof(container));" - print "\t\tbool updated;" - print "\t\tfor(uint32_t i = 0;i #include @@ -48,22 +47,24 @@ #include #include +#include using namespace std; extern void px4_show_devices(void); extern "C" { -""" +""") for app in apps: - print "extern int "+app+"_main(int argc, char *argv[]);" + print("extern int "+app+"_main(int argc, char *argv[]);") -print """ +print(""" static int shutdown_main(int argc, char *argv[]); static int list_tasks_main(int argc, char *argv[]); static int list_files_main(int argc, char *argv[]); static int list_devices_main(int argc, char *argv[]); static int list_topics_main(int argc, char *argv[]); +static int sleep_main(int argc, char *argv[]); } @@ -72,16 +73,17 @@ static map app_map(void) { static map apps; -""" +""") for app in apps: - print '\tapps["'+app+'"] = '+app+'_main;' - -print '\tapps["shutdown"] = shutdown_main;' -print '\tapps["list_tasks"] = list_tasks_main;' -print '\tapps["list_files"] = list_files_main;' -print '\tapps["list_devices"] = list_devices_main;' -print '\tapps["list_topics"] = list_topics_main;' -print """ + print('\tapps["'+app+'"] = '+app+'_main;') + +print('\tapps["shutdown"] = shutdown_main;') +print('\tapps["list_tasks"] = list_tasks_main;') +print('\tapps["list_files"] = list_files_main;') +print('\tapps["list_devices"] = list_devices_main;') +print('\tapps["list_topics"] = list_topics_main;') +print('\tapps["sleep"] = sleep_main;') +print(""" return apps; } @@ -96,7 +98,9 @@ static int shutdown_main(int argc, char *argv[]) { - cout << "Shutting down" << endl; + cout.flush(); + cout << endl << "Shutting down" << endl; + cout.flush(); exit(0); } @@ -122,5 +126,23 @@ px4_show_files(); return 0; } -""" +static int sleep_main(int argc, char *argv[]) +{ + if (argc != 2) { + cout << "Usage: sleep " << endl; + return 1; + } + sleep(atoi(argv[1])); + return 0; +} +static int usleep_main(int argc, char *argv[]) +{ + if (argc != 2) { + cout << "Usage: usleep " << endl; + return 1; + } + usleep(atoi(argv[1])); + return 0; +} +""") diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 048836a4ebd5..b67ad890a985 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -1,5 +1,7 @@ import sys import re +global default_var +default_var = {} class ParameterGroup(object): """ @@ -43,6 +45,7 @@ class Parameter(object): "min": 5, "max": 4, "unit": 3, + "decimal": 2, # all others == 0 (sorted alphabetically) } @@ -98,11 +101,13 @@ class SourceParser(object): re_comment_end = re.compile(r'(.*?)\s*\*\/') re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') re_px4_parameter_definition = re.compile(r'PX4_PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*\)\s*;') + re_px4_param_default_definition = re.compile(r'#define\s*PARAM_([A-Z_][A-Z0-9_]*)\s*([^ ,\)]+)\s*') re_cut_type_specifier = re.compile(r'[a-z]+$') re_is_a_number = re.compile(r'^-?[0-9\.]') re_remove_dots = re.compile(r'\.+$') + re_remove_carriage_return = re.compile('\n+') - valid_tags = set(["group", "board", "min", "max", "unit"]) + valid_tags = set(["group", "board", "min", "max", "unit", "decimal"]) # Order of parameter groups priority = { @@ -113,13 +118,6 @@ class SourceParser(object): def __init__(self): self.param_groups = {} - def GetSupportedExtensions(self): - """ - Returns list of supported file extensions that can be parsed by this - parser. - """ - return [".cpp", ".c"] - def Parse(self, contents): """ Incrementally parse program contents and append all found parameters @@ -188,12 +186,26 @@ def Parse(self, contents): if last_comment_line: state = "comment-processed" else: + tp = None + name = None + defval = "" # Non-empty line outside the comment + m = self.re_px4_param_default_definition.match(line) + if m: + name_m, defval_m = m.group(1,2) + default_var[name_m] = defval_m m = self.re_parameter_definition.match(line) if m: tp, name, defval = m.group(1, 2, 3) + else: + m = self.re_px4_parameter_definition.match(line) + if m: + tp, name = m.group(1, 2) + if (name+'_DEFAULT') in default_var: + defval = default_var[name+'_DEFAULT'] + if tp is not None: # Remove trailing type specifier from numbers: 0.1f => 0.1 - if self.re_is_a_number.match(defval): + if defval != "" and self.re_is_a_number.match(defval): defval = self.re_cut_type_specifier.sub('', defval) param = Parameter(name, tp, defval) param.SetField("short_desc", name) @@ -202,52 +214,71 @@ def Parse(self, contents): group = "Miscellaneous" if state == "comment-processed": if short_desc is not None: - param.SetField("short_desc", - self.re_remove_dots.sub('', short_desc)) + param.SetField("short_desc", self.re_remove_dots.sub('', short_desc)) if long_desc is not None: + long_desc = self.re_remove_carriage_return.sub(' ', long_desc) param.SetField("long_desc", long_desc) for tag in tags: if tag == "group": group = tags[tag] elif tag not in self.valid_tags: - sys.stderr.write("Skipping invalid " - "documentation tag: '%s'\n" % tag) + sys.stderr.write("Skipping invalid documentation tag: '%s'\n" % tag) + return False else: param.SetField(tag, tags[tag]) # Store the parameter if group not in self.param_groups: self.param_groups[group] = ParameterGroup(group) self.param_groups[group].AddParameter(param) - else: - # Nasty code dup, but this will all go away soon, so quick and dirty (DonLakeFlyer) - m = self.re_px4_parameter_definition.match(line) - if m: - tp, name = m.group(1, 2) - param = Parameter(name, tp) - param.SetField("short_desc", name) - # If comment was found before the parameter declaration, - # inject its data into the newly created parameter. - group = "Miscellaneous" - if state == "comment-processed": - if short_desc is not None: - param.SetField("short_desc", - self.re_remove_dots.sub('', short_desc)) - if long_desc is not None: - param.SetField("long_desc", long_desc) - for tag in tags: - if tag == "group": - group = tags[tag] - elif tag not in self.valid_tags: - sys.stderr.write("Skipping invalid " - "documentation tag: '%s'\n" % tag) - else: - param.SetField(tag, tags[tag]) - # Store the parameter - if group not in self.param_groups: - self.param_groups[group] = ParameterGroup(group) - self.param_groups[group].AddParameter(param) - # Reset parsed comment. - state = None + state = None + return True + + def IsNumber(self, numberString): + try: + float(numberString) + return True + except ValueError: + return False + + def Validate(self): + """ + Validates the parameter meta data. + """ + seenParamNames = [] + for group in self.GetParamGroups(): + for param in group.GetParams(): + name = param.GetName() + board = param.GetFieldValue("board") + # Check for duplicates + name_plus_board = name + "+" + board + for seenParamName in seenParamNames: + if seenParamName == name_plus_board: + sys.stderr.write("Duplicate parameter definition: {0}\n".format(name_plus_board)) + return False + seenParamNames.append(name_plus_board) + # Validate values + default = param.GetDefault() + min = param.GetFieldValue("min") + max = param.GetFieldValue("max") + #sys.stderr.write("{0} default:{1} min:{2} max:{3}\n".format(name, default, min, max)) + if default != "" and not self.IsNumber(default): + sys.stderr.write("Default value not number: {0} {1}\n".format(name, default)) + return False + if min != "": + if not self.IsNumber(min): + sys.stderr.write("Min value not number: {0} {1}\n".format(name, min)) + return False + if default != "" and float(default) < float(min): + sys.stderr.write("Default value is smaller than min: {0} default:{1} min:{2}\n".format(name, default, min)) + return False + if max != "": + if not self.IsNumber(max): + sys.stderr.write("Max value not number: {0} {1}\n".format(name, max)) + return False + if default != "" and float(default) > float(max): + sys.stderr.write("Default value is larger than max: {0} default:{1} max:{2}\n".format(name, default, max)) + return False + return True def GetParamGroups(self): """ diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py index 1f0ea4e89e4a..4db5698992dc 100644 --- a/Tools/px4params/srcscanner.py +++ b/Tools/px4params/srcscanner.py @@ -13,12 +13,20 @@ def ScanDir(self, srcdir, parser): Scans provided path and passes all found contents to the parser using parser.Parse method. """ - extensions = tuple(parser.GetSupportedExtensions()) + extensions1 = tuple([".h"]) + extensions2 = tuple([".cpp", ".c"]) for dirname, dirnames, filenames in os.walk(srcdir): for filename in filenames: - if filename.endswith(extensions): - path = os.path.join(dirname, filename) - self.ScanFile(path, parser) + if filename.endswith(extensions1): + path = os.path.join(dirname, filename) + if not self.ScanFile(path, parser): + return False + for filename in filenames: + if filename.endswith(extensions2): + path = os.path.join(dirname, filename) + if not self.ScanFile(path, parser): + return False + return True def ScanFile(self, path, parser): """ @@ -32,4 +40,4 @@ def ScanFile(self, path, parser): contents = '' print('Failed reading file: %s, skipping content.' % path) pass - parser.Parse(contents) + return parser.Parse(contents) diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index b072ab79f8e4..b37cdb062782 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -18,10 +18,14 @@ def indent(elem, level=0): class XMLOutput(): - def __init__(self, groups, board): + def __init__(self, groups, board, inject_xml_file_name): xml_parameters = ET.Element("parameters") xml_version = ET.SubElement(xml_parameters, "version") xml_version.text = "3" + importtree = ET.parse(inject_xml_file_name) + injectgroups = importtree.getroot().findall("group") + for igroup in injectgroups: + xml_parameters.append(igroup) last_param_name = "" board_specific_param_set = False for group in groups: diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 75acd6a60847..7ed0b3af19df 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -43,6 +43,11 @@ import filecmp import argparse +import sys +px4_tools_dir = os.path.dirname(os.path.abspath(__file__)) +sys.path.append(px4_tools_dir + "/genmsg/src") +sys.path.append(px4_tools_dir + "/gencpp/src") + try: import genmsg.template_tools except ImportError as e: @@ -79,7 +84,7 @@ def convert_file(filename, outputdir, templatedir, includepath): """ Converts a single .msg file to a uorb header """ - print("Generating headers from {0}".format(filename)) + #print("Generating headers from {0}".format(filename)) genmsg.template_tools.generate_from_file(filename, package, outputdir, @@ -93,6 +98,30 @@ def convert_dir(inputdir, outputdir, templatedir): """ Converts all .msg files in inputdir to uORB header files """ + + # Find the most recent modification time in input dir + maxinputtime = 0 + for f in os.listdir(inputdir): + fni = os.path.join(inputdir, f) + if os.path.isfile(fni): + it = os.path.getmtime(fni) + if it > maxinputtime: + maxinputtime = it; + + # Find the most recent modification time in output dir + maxouttime = 0 + if os.path.isdir(outputdir): + for f in os.listdir(outputdir): + fni = os.path.join(outputdir, f) + if os.path.isfile(fni): + it = os.path.getmtime(fni) + if it > maxouttime: + maxouttime = it; + + # Do not generate if nothing changed on the input + if (maxinputtime != 0 and maxouttime != 0 and maxinputtime < maxouttime): + return False + includepath = incl_default + [':'.join([package, inputdir])] for f in os.listdir(inputdir): # Ignore hidden files @@ -109,8 +138,10 @@ def convert_dir(inputdir, outputdir, templatedir): templatedir, includepath) + return True + -def copy_changed(inputdir, outputdir, prefix=''): +def copy_changed(inputdir, outputdir, prefix='', quiet=False): """ Copies files from inputdir to outputdir if they don't exist in ouputdir or if their content changed @@ -127,28 +158,32 @@ def copy_changed(inputdir, outputdir, prefix=''): fno = os.path.join(outputdir, prefix + f) if not os.path.isfile(fno): shutil.copy(fni, fno) - print("{0}: new header file".format(f)) - continue - # The file exists in inputdir and outputdir - # only copy if contents do not match - if not filecmp.cmp(fni, fno): - shutil.copy(fni, fno) - print("{0}: updated".format(f)) + if not quiet: + print("{0}: new header file".format(f)) continue - print("{0}: unchanged".format(f)) + if os.path.getmtime(fni) > os.path.getmtime(fno): + # The file exists in inputdir and outputdir + # only copy if contents do not match + if not filecmp.cmp(fni, fno): + shutil.copy(fni, fno) + if not quiet: + print("{0}: updated".format(f)) + continue + + if not quiet: + print("{0}: unchanged".format(f)) -def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix): +def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix, quiet=False): """ Converts all .msg files in inputdir to uORB header files Unchanged existing files are not overwritten. """ # Create new headers in temporary output directory convert_dir(inputdir, temporarydir, templatedir) - # Copy changed headers from temporary dir to output dir - copy_changed(temporarydir, outputdir, prefix) + copy_changed(temporarydir, outputdir, prefix, quiet) if __name__ == "__main__": parser = argparse.ArgumentParser( @@ -166,6 +201,9 @@ def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix): parser.add_argument('-p', dest='prefix', default='', help='string added as prefix to the output file ' ' name when converting directories') + parser.add_argument('-q', dest='quiet', default=False, action='store_true', + help='string added as prefix to the output file ' + ' name when converting directories') args = parser.parse_args() if args.file is not None: @@ -181,4 +219,5 @@ def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix): args.outputdir, args.templatedir, args.temporarydir, - args.prefix) + args.prefix, + args.quiet) diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index 152444f84b6b..2f07fa1e73e8 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -74,6 +74,7 @@ def mkdesc(): 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("--airframe_xml", action="store", help="the airframes.xml file") parser.add_argument("--image", action="store", help="the firmware image") args = parser.parse_args() @@ -107,6 +108,11 @@ def mkdesc(): bytes = f.read() desc['parameter_xml_size'] = len(bytes) desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') +if args.airframe_xml != None: + f = open(args.airframe_xml, "rb") + bytes = f.read() + desc['airframe_xml_size'] = len(bytes) + desc['airframe_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/Tools/px_process_params.py b/Tools/px_process_params.py index cb2202d5296f..fcd5b994e98b 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -65,6 +65,12 @@ def main(): metavar="FILENAME", help="Create XML file" " (default FILENAME: parameters.xml)") + parser.add_argument("-i", "--inject-xml", + nargs='?', + const="../Tools/parameters_injected.xml", + metavar="FILENAME", + help="Inject additional param XML file" + " (default FILENAME: ../Tools/parameters_injected.xml)") parser.add_argument("-b", "--board", nargs='?', const="", @@ -115,13 +121,16 @@ def main(): # Scan directories, and parse the files print("Scanning source path " + args.src_path) - scanner.ScanDir(args.src_path, parser) + if not scanner.ScanDir(args.src_path, parser): + sys.exit(1) + if not parser.Validate(): + sys.exit(1) param_groups = parser.GetParamGroups() # Output to XML file if args.xml: print("Creating XML file " + args.xml) - out = xmlout.XMLOutput(param_groups, args.board) + out = xmlout.XMLOutput(param_groups, args.board, os.path.join(args.src_path, args.inject_xml)) out.Save(args.xml) # Output to DokuWiki tables diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index aef1cc7a3b35..a7984c66dd94 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -40,7 +40,7 @@ """ from __future__ import print_function -import argparse +import argparse, re import os @@ -57,14 +57,14 @@ def main(): for (root, dirs, files) in os.walk(args.folder): for file in files: # only prune text files - if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file: + if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file or ".DS_Store" in file: continue file_path = os.path.join(root, file) # read file line by line pruned_content = "" - with open(file_path, "r") as f: + with open(file_path, "rU") as f: for line in f: # handle mixer files differently than startup files @@ -74,10 +74,10 @@ def main(): else: if not line.isspace() and not line.strip().startswith("#"): pruned_content += line - - # overwrite old scratch file - with open(file_path, "w") as f: - f.write(pruned_content) + # overwrite old scratch file + with open(file_path, "wb") as f: + pruned_content = re.sub("\r\n", "\n", pruned_content) + f.write(pruned_content.encode("ascii", errors='strict')) if __name__ == '__main__': diff --git a/Tools/qurt_apps.py b/Tools/qurt_apps.py index e5d75344beb1..b1f606c37a62 100755 --- a/Tools/qurt_apps.py +++ b/Tools/qurt_apps.py @@ -48,6 +48,8 @@ #include #include +#include +#include using namespace std; @@ -64,6 +66,7 @@ static int list_files_main(int argc, char *argv[]); static int list_devices_main(int argc, char *argv[]); static int list_topics_main(int argc, char *argv[]); +static int sleep_main(int argc, char *argv[]); } @@ -78,6 +81,7 @@ print '\tapps["list_files"] = list_files_main;' print '\tapps["list_devices"] = list_devices_main;' print '\tapps["list_topics"] = list_topics_main;' +print '\tapps["sleep"] = sleep_main;' print """ } @@ -117,5 +121,17 @@ px4_show_files(); return 0; } +static int sleep_main(int argc, char *argv[]) +{ + if (argc != 2) { + PX4_WARN( "Usage: sleep " ); + return 1; + } + + unsigned long usecs = ( (unsigned long) atol( argv[1] ) ) * 1000 * 1000; + PX4_WARN("Sleeping for %s, %ld",argv[1],usecs); + usleep( usecs ); + return 0; +} """ diff --git a/Tools/sdlog2/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py index aeb60139c5e9..8b0ccb2d7de3 100755 --- a/Tools/sdlog2/sdlog2_dump.py +++ b/Tools/sdlog2/sdlog2_dump.py @@ -46,7 +46,6 @@ class SDLog2Parser: "h": ("h", None), "H": ("H", None), "i": ("i", None), - "d": ("d", None), "I": ("I", None), "f": ("f", None), "n": ("4s", None), From f16b74d29f907597c3d6b1821cdfc83e8b26f94e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:11:10 +1100 Subject: [PATCH 247/293] launch: update from upstream --- launch/ardrone.launch | 2 ++ launch/iris.launch | 2 ++ launch/multicopter.launch | 5 ++++- launch/multicopter_w.launch | 2 ++ launch/multicopter_x.launch | 2 ++ 5 files changed, 12 insertions(+), 1 deletion(-) diff --git a/launch/ardrone.launch b/launch/ardrone.launch index f43bbf47092a..db980731fe56 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -1,8 +1,10 @@ + + diff --git a/launch/iris.launch b/launch/iris.launch index 5231e3215b8e..798b761d76dd 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -1,8 +1,10 @@ + + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 6882f241378d..bf68da95ddef 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -1,5 +1,6 @@ + @@ -10,7 +11,9 @@ - + + + diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch index 66c30d186faf..617357452f95 100644 --- a/launch/multicopter_w.launch +++ b/launch/multicopter_w.launch @@ -1,8 +1,10 @@ + + diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch index c686eba390bd..b7db46dcb5c8 100644 --- a/launch/multicopter_x.launch +++ b/launch/multicopter_x.launch @@ -1,8 +1,10 @@ + + From 61cfe7afd0d588ee1a369569d7005fa401376ab7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:11:25 +1100 Subject: [PATCH 248/293] drivers: update from upstream --- src/drivers/airspeed/airspeed.cpp | 75 +- .../ardrone_interface/ardrone_interface.c | 51 +- .../ardrone_interface/ardrone_motor_control.c | 54 +- src/drivers/blinkm/blinkm.cpp | 256 +++-- src/drivers/bma180/bma180.cpp | 122 ++- src/drivers/boards/aerocore/aerocore_init.c | 12 +- src/drivers/boards/aerocore/aerocore_led.c | 14 +- src/drivers/boards/aerocore/aerocore_spi.c | 6 +- src/drivers/boards/aerocore/board_config.h | 15 + src/drivers/hil/hil.cpp | 892 ------------------ src/drivers/hil/module.mk | 42 - 11 files changed, 382 insertions(+), 1157 deletions(-) delete mode 100644 src/drivers/hil/hil.cpp delete mode 100644 src/drivers/hil/module.mk diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index f2352648dc6f..df7f8c633a99 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,8 +32,9 @@ ****************************************************************************/ /** - * @file ets_airspeed.cpp - * @author Simon Wilks + * @file airspeed.cpp + * @author Simon Wilks + * @author Lorenz Meier * * Driver for the Eagle Tree Airspeed V3 connected via I2C. */ @@ -76,7 +77,7 @@ #include -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : +Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) : I2C("Airspeed", path, bus, address, 100000), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), @@ -105,12 +106,14 @@ Airspeed::~Airspeed() /* make sure we are truly inactive */ stop(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -124,13 +127,16 @@ Airspeed::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::RingBuffer(2, sizeof(differential_pressure_s)); - if (_reports == nullptr) + + if (_reports == nullptr) { goto out; + } /* register alternate interfaces if we have to */ _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); @@ -146,8 +152,9 @@ Airspeed::init() /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); - if (_airspeed_pub == nullptr) + if (_airspeed_pub == nullptr) { warnx("uORB started?"); + } } ret = OK; @@ -166,7 +173,7 @@ Airspeed::probe() _retries = 4; int ret = measure(); - // drop back to 2 retries once initialised + // drop back to 2 retries once initialised _retries = 2; return ret; } @@ -179,20 +186,20 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -202,13 +209,14 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -217,15 +225,17 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(_conversion_interval)) + if (ticks < USEC2TICK(_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) + if (want_start) { start(); + } return OK; } @@ -233,21 +243,25 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) } 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)) + if ((arg < 1) || (arg > 100)) { return -EINVAL; + } irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } + irqrestore(flags); return OK; @@ -261,16 +275,16 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - _diff_pres_offset = s->offset_pa; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + _diff_pres_offset = s->offset_pa; + return OK; } case AIRSPEEDIOCGSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - s->offset_pa = _diff_pres_offset; - s->scale = 1.0f; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + s->offset_pa = _diff_pres_offset; + s->scale = 1.0f; + return OK; } default: @@ -287,8 +301,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) 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) { @@ -369,6 +384,7 @@ Airspeed::update_status() if (_subsys_pub != nullptr) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -402,6 +418,7 @@ Airspeed::print_info() void Airspeed::new_report(const differential_pressure_s &report) { - if (!_reports->force(&report)) + if (!_reports->force(&report)) { perf_count(_buffer_overflows); + } } diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 1777e7f27c03..8ad0ee716889 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -92,8 +92,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { warnx("%s\n", reason); + } + warnx("usage: {start|stop|status} [-d ]\n\n"); exit(1); } @@ -122,11 +124,11 @@ int ardrone_interface_main(int argc, char *argv[]) thread_should_exit = false; ardrone_interface_task = px4_task_spawn_cmd("ardrone_interface", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 1100, - ardrone_interface_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 1100, + ardrone_interface_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } @@ -138,9 +140,11 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("running"); + } else { warnx("not started"); } + exit(0); } @@ -212,19 +216,23 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) { - if (i+1 < argc) { - int motor = atoi(argv[i+1]); + if (i + 1 < argc) { + int motor = atoi(argv[i + 1]); + if (motor > 0 && motor < 5) { test_motor = motor; + } else { thread_running = false; errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage); } + } else { thread_running = false; errx(1, "missing parameter to -m 1..4\n %s", commandline_usage); } } + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set if (argc > i + 1) { device = argv[i + 1]; @@ -314,6 +322,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int motors[4] = {0, 0, 0, 0}; motors[test_motor - 1] = 10; ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]); + } else { ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10); } @@ -338,33 +347,33 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } if (counter % 24 == 0) { - if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); + if (led_counter == 0) { ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); } - if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); + if (led_counter == 1) { ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); } - if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); + if (led_counter == 2) { ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); } - if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); + if (led_counter == 3) { ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); } - if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); + if (led_counter == 4) { ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); } - if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); + if (led_counter == 5) { ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); } - if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); + if (led_counter == 6) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); } - if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); + if (led_counter == 7) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); } - if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); + if (led_counter == 8) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); } - if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); + if (led_counter == 9) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); } - if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); + if (led_counter == 10) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); } - if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); + if (led_counter == 11) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); } led_counter++; - if (led_counter == 12) led_counter = 0; + if (led_counter == 12) { led_counter = 0; } } /* run at approximately 200 Hz */ diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 46e1f86511fb..499138912942 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -108,7 +108,7 @@ void ar_enable_broadcast(int fd) int ar_multiplexing_init() { int fd; - + fd = open(PX4FMU_DEVICE_PATH, 0); if (fd < 0) { @@ -176,7 +176,7 @@ int ar_select_motor(int fd, uint8_t motor) ret += ioctl(fd, GPIO_CLEAR, motor_gpios); } else { - /* select reqested motor */ + /* select reqested motor */ ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); } @@ -199,7 +199,7 @@ int ar_deselect_motor(int fd, uint8_t motor) ret += ioctl(fd, GPIO_SET, motor_gpios); } else { - /* deselect reqested motor */ + /* deselect reqested motor */ ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]); } @@ -235,7 +235,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[0]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*1); + usleep(UART_TRANSFER_TIME_BYTE_US * 1); /* * write 0x91 - request checksum @@ -243,7 +243,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[1]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*120); + usleep(UART_TRANSFER_TIME_BYTE_US * 120); /* * write 0xA1 - set status OK @@ -252,7 +252,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[2]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*1); + usleep(UART_TRANSFER_TIME_BYTE_US * 1); /* * set as motor i, where i = 1..4 @@ -268,7 +268,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[4]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*11); + usleep(UART_TRANSFER_TIME_BYTE_US * 11); ar_deselect_motor(gpios, i); /* sleep 200 ms */ @@ -304,13 +304,15 @@ int ar_init_motors(int ardrone_uart, int gpios) warnx("Failed %d times", -errcounter); fflush(stdout); } + return errcounter; } /** * Sets the leds on the motor controllers, 1 turns led on, 0 off. */ -void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, + uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) { /* * 2 bytes are sent. The first 3 bits describe the command: 011 means led control @@ -322,12 +324,15 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t * 011 rrrr 0000 gggg 0 */ uint8_t leds[2]; - leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1); - leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1); + leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | (( + led1_red & 0x01) << 1); + leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | (( + led1_green & 0x01) << 1); write(ardrone_uart, leds, 2); } -int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) { +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) +{ const unsigned int min_motor_interval = 4900; static uint64_t last_motor_time = 0; @@ -338,6 +343,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor outputs.output[2] = motor3; outputs.output[3] = motor4; static orb_advert_t pub = 0; + if (pub == 0) { /* advertise to channel 0 / primary */ pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); @@ -355,15 +361,18 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor if (ret == sizeof(buf)) { return OK; + } else { return ret; } + } else { return -ERROR; } } -void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) { +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) +{ float roll_control = actuators->control[0]; float pitch_control = actuators->control[1]; @@ -385,7 +394,8 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls /* linearly scale the control inputs from 0 to startpoint_full_control */ if (motor_thrust < startpoint_full_control) { - output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1 + output_band = motor_thrust / startpoint_full_control; // linear from 0 to 1 + } else { output_band = 1.0f; } @@ -407,7 +417,7 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); /* if one motor is saturated, reduce throttle */ - float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust; + float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]), fmaxf(motor_calc[2], motor_calc[3])) - max_thrust; if (saturation > 0.0f) { @@ -428,16 +438,16 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls /* set the motor values */ /* scale up from 0..1 to 10..500) */ - motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[0] = (uint16_t)(motor_calc[0] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[1] = (uint16_t)(motor_calc[1] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[2] = (uint16_t)(motor_calc[2] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[3] = (uint16_t)(motor_calc[3] * ((float)max_gas - min_gas) + min_gas); /* scale up from 0..1 to 10..500) */ - motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[0] = (uint16_t)(motor_calc[0] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[1] = (uint16_t)(motor_calc[1] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[2] = (uint16_t)(motor_calc[2] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[3] = (uint16_t)(motor_calc[3] * (float)((max_gas - min_gas) + min_gas)); /* Failsafe logic for min values - should never be necessary */ motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas; diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 185565d8b6db..78fce9162efc 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -244,7 +244,7 @@ class BlinkM : public device::I2C /* for now, we only support one BlinkM */ namespace { - BlinkM *g_blinkm; +BlinkM *g_blinkm; } /* list of script names, must match script ID numbers */ @@ -277,9 +277,9 @@ extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); BlinkM::BlinkM(int bus, int blinkm) : I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm #ifdef __PX4_NUTTX - , 100000 + , 100000 #endif - ), + ), led_color_1(LED_OFF), led_color_2(LED_OFF), led_color_3(LED_OFF), @@ -325,7 +325,7 @@ BlinkM::init() } stop_script(); - set_rgb(0,0,0); + set_rgb(0, 0, 0); return OK; } @@ -333,13 +333,14 @@ BlinkM::init() int BlinkM::setMode(int mode) { - if(mode == 1) { - if(systemstate_run == false) { + if (mode == 1) { + if (systemstate_run == false) { stop_script(); - set_rgb(0,0,0); + set_rgb(0, 0, 0); systemstate_run = true; work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); } + } else { systemstate_run = false; } @@ -355,8 +356,9 @@ BlinkM::probe() ret = get_firmware_version(version); - if (ret == OK) - log("found BlinkM firmware version %c%c", version[1], version[0]); + if (ret == OK) { + DEVICE_DEBUG("found BlinkM firmware version %c%c", version[1], version[0]); + } return ret; } @@ -372,6 +374,7 @@ BlinkM::ioctl(device::file_t *filp, int cmd, unsigned long arg) ret = EINVAL; break; } + ret = play_script((const char *)arg); break; @@ -380,24 +383,30 @@ BlinkM::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; case BLINKM_SET_USER_SCRIPT: { - if (arg == 0) { - ret = EINVAL; - break; - } + if (arg == 0) { + ret = EINVAL; + break; + } - unsigned lines = 0; - const uint8_t *script = (const uint8_t *)arg; + unsigned lines = 0; + const uint8_t *script = (const uint8_t *)arg; - while ((lines < 50) && (script[1] != 0)) { - ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]); - if (ret != OK) - break; - script += 5; + while ((lines < 50) && (script[1] != 0)) { + ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]); + + if (ret != OK) { + break; + } + + script += 5; + } + + if (ret == OK) { + ret = set_script(lines, 0); + } + + break; } - if (ret == OK) - ret = set_script(lines, 0); - break; - } default: break; @@ -421,7 +430,7 @@ void BlinkM::led() { - if(!topic_initialized) { + if (!topic_initialized) { vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 250); @@ -441,29 +450,36 @@ BlinkM::led() topic_initialized = true; } - if(led_thread_ready == true) { - if(!detected_cells_blinked) { - if(num_of_cells > 0) { + if (led_thread_ready == true) { + if (!detected_cells_blinked) { + if (num_of_cells > 0) { t_led_color[0] = LED_PURPLE; } - if(num_of_cells > 1) { + + if (num_of_cells > 1) { t_led_color[1] = LED_PURPLE; } - if(num_of_cells > 2) { + + if (num_of_cells > 2) { t_led_color[2] = LED_PURPLE; } - if(num_of_cells > 3) { + + if (num_of_cells > 3) { t_led_color[3] = LED_PURPLE; } - if(num_of_cells > 4) { + + if (num_of_cells > 4) { t_led_color[4] = LED_PURPLE; } - if(num_of_cells > 5) { + + if (num_of_cells > 5) { t_led_color[5] = LED_PURPLE; } + t_led_color[6] = LED_OFF; t_led_color[7] = LED_OFF; t_led_blink = LED_BLINK; + } else { t_led_color[0] = led_color_1; t_led_color[1] = led_color_2; @@ -475,13 +491,17 @@ BlinkM::led() t_led_color[7] = led_color_8; t_led_blink = led_blink; } + led_thread_ready = false; } if (led_thread_runcount & 1) { - if (t_led_blink) + if (t_led_blink) { setLEDColor(LED_OFF); + } + led_interval = LED_OFFTIME; + } else { setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]); //led_interval = (led_thread_runcount & 1) : LED_ONTIME; @@ -516,10 +536,13 @@ BlinkM::led() if (new_data_vehicle_status) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); no_data_vehicle_status = 0; + } else { no_data_vehicle_status++; - if(no_data_vehicle_status >= 3) + + if (no_data_vehicle_status >= 3) { no_data_vehicle_status = 3; + } } orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode); @@ -527,10 +550,13 @@ BlinkM::led() if (new_data_vehicle_control_mode) { orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode); no_data_vehicle_control_mode = 0; + } else { no_data_vehicle_control_mode++; - if(no_data_vehicle_control_mode >= 3) + + if (no_data_vehicle_control_mode >= 3) { no_data_vehicle_control_mode = 3; + } } orb_check(actuator_armed_sub_fd, &new_data_actuator_armed); @@ -538,10 +564,13 @@ BlinkM::led() if (new_data_actuator_armed) { orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed); no_data_actuator_armed = 0; + } else { no_data_actuator_armed++; - if(no_data_actuator_armed >= 3) + + if (no_data_actuator_armed >= 3) { no_data_actuator_armed = 3; + } } orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); @@ -549,10 +578,13 @@ BlinkM::led() if (new_data_vehicle_gps_position) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); no_data_vehicle_gps_position = 0; + } else { no_data_vehicle_gps_position++; - if(no_data_vehicle_gps_position >= 3) + + if (no_data_vehicle_gps_position >= 3) { no_data_vehicle_gps_position = 3; + } } /* update safety topic */ @@ -569,13 +601,15 @@ BlinkM::led() if (num_of_cells == 0) { /* looking for lipo cells that are connected */ printf(" checking cells\n"); - for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; + + for (num_of_cells = 2; num_of_cells < 7; num_of_cells++) { + if (vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) { break; } } + printf(" cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { + if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -587,7 +621,7 @@ BlinkM::led() led_color_8 = LED_RED; led_blink = LED_BLINK; - } else if(vehicle_status_raw.rc_signal_lost) { + } else if (vehicle_status_raw.rc_signal_lost) { /* LED Pattern for FAILSAFE */ led_color_1 = LED_BLUE; led_color_2 = LED_BLUE; @@ -599,7 +633,7 @@ BlinkM::led() led_color_8 = LED_BLUE; led_blink = LED_BLINK; - } else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { + } else if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; @@ -614,9 +648,9 @@ BlinkM::led() } else { /* no battery warnings here */ - if(actuator_armed.armed == false) { + if (actuator_armed.armed == false) { /* system not armed */ - if(safety.safety_off){ + if (safety.safety_off) { led_color_1 = LED_ORANGE; led_color_2 = LED_ORANGE; led_color_3 = LED_ORANGE; @@ -626,7 +660,8 @@ BlinkM::led() led_color_7 = LED_ORANGE; led_color_8 = LED_ORANGE; led_blink = LED_BLINK; - }else{ + + } else { led_color_1 = LED_CYAN; led_color_2 = LED_CYAN; led_color_3 = LED_CYAN; @@ -637,6 +672,7 @@ BlinkM::led() led_color_8 = LED_CYAN; led_blink = LED_NOBLINK; } + } else { /* armed system - initial led pattern */ led_color_1 = LED_RED; @@ -649,32 +685,41 @@ BlinkM::led() led_color_8 = LED_OFF; led_blink = LED_BLINK; - if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { + if (new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) + if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) { led_color_4 = LED_GREEN; + } + /* TODO: add other Auto modes */ - else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) { led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) + + } else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) { led_color_4 = LED_YELLOW; - else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) + + } else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) { led_color_4 = LED_WHITE; - else + + } else { led_color_4 = LED_OFF; + } + led_color_5 = led_color_4; } - if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { + if (new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { /* handling used satus */ - if(num_of_used_sats >= 7) { + if (num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; led_color_3 = LED_OFF; - } else if(num_of_used_sats == 6) { + + } else if (num_of_used_sats == 6) { led_color_2 = LED_OFF; led_color_3 = LED_OFF; - } else if(num_of_used_sats == 5) { + + } else if (num_of_used_sats == 5) { led_color_3 = LED_OFF; } @@ -689,6 +734,7 @@ BlinkM::led() } } } + } else { /* LED Pattern for general Error - no vehicle_status can retrieved */ led_color_1 = LED_WHITE; @@ -716,12 +762,13 @@ BlinkM::led() vehicle_gps_position_raw.satellites_visible); */ - led_thread_runcount=0; + led_thread_runcount = 0; led_thread_ready = true; led_interval = LED_OFFTIME; - if(detected_cells_runcount < 4){ + if (detected_cells_runcount < 4) { detected_cells_runcount++; + } else { detected_cells_blinked = true; } @@ -730,47 +777,58 @@ BlinkM::led() led_thread_runcount++; } - if(systemstate_run == true) { + if (systemstate_run == true) { /* re-queue ourselves to run again later */ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); + } else { stop_script(); - set_rgb(0,0,0); + set_rgb(0, 0, 0); } } -void BlinkM::setLEDColor(int ledcolor) { +void BlinkM::setLEDColor(int ledcolor) +{ switch (ledcolor) { - case LED_OFF: // off - set_rgb(0,0,0); - break; - case LED_RED: // red - set_rgb(255,0,0); - break; - case LED_ORANGE: // orange - set_rgb(255,150,0); - break; - case LED_YELLOW: // yellow - set_rgb(200,200,0); - break; - case LED_PURPLE: // purple - set_rgb(255,0,255); - break; - case LED_GREEN: // green - set_rgb(0,255,0); - break; - case LED_BLUE: // blue - set_rgb(0,0,255); - break; - case LED_CYAN: // cyan - set_rgb(0,128,128); - break; - case LED_WHITE: // white - set_rgb(255,255,255); - break; - case LED_AMBER: // amber - set_rgb(255,65,0); - break; + case LED_OFF: // off + set_rgb(0, 0, 0); + break; + + case LED_RED: // red + set_rgb(255, 0, 0); + break; + + case LED_ORANGE: // orange + set_rgb(255, 150, 0); + break; + + case LED_YELLOW: // yellow + set_rgb(200, 200, 0); + break; + + case LED_PURPLE: // purple + set_rgb(255, 0, 255); + break; + + case LED_GREEN: // green + set_rgb(0, 255, 0); + break; + + case LED_BLUE: // blue + set_rgb(0, 0, 255); + break; + + case LED_CYAN: // cyan + set_rgb(0, 128, 128); + break; + + case LED_WHITE: // white + set_rgb(255, 255, 255); + break; + + case LED_AMBER: // amber + set_rgb(255, 65, 0); + break; } } @@ -855,8 +913,9 @@ BlinkM::play_script(const char *script_name) } for (unsigned i = 0; script_names[i] != nullptr; i++) - if (!strcasecmp(script_name, script_names[i])) + if (!strcasecmp(script_name, script_names[i])) { return play_script(i); + } return -1; } @@ -931,7 +990,8 @@ BlinkM::get_firmware_version(uint8_t version[2]) void blinkm_usage(); -void blinkm_usage() { +void blinkm_usage() +{ warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}"); warnx("options:"); warnx("\t-b --bus i2cbus (3)"); @@ -951,6 +1011,7 @@ blinkm_main(int argc, char *argv[]) blinkm_usage(); return 1; } + for (x = 1; x < argc; x++) { if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { if (argc > x + 1) { @@ -1008,22 +1069,27 @@ blinkm_main(int argc, char *argv[]) if (!strcmp(argv[1], "list")) { - for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) + for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) { fprintf(stderr, " %s\n", BlinkM::script_names[i]); + } + fprintf(stderr, " \n"); return 0; } /* things that require access to the device */ int fd = px4_open(BLINKM0_DEVICE_PATH, 0); + if (fd < 0) { warn("can't open BlinkM device"); return 1; } g_blinkm->setMode(0); - if (px4_ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) + + if (px4_ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) { return 0; + } px4_close(fd); diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index d83cfeb4d4bb..6557746c1a0d 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -262,8 +262,9 @@ BMA180::~BMA180() stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } /* delete the perf counter */ perf_free(_sample_perf); @@ -275,14 +276,16 @@ BMA180::init() int ret = ERROR; /* do SPI init (and probe) first */ - if (SPI::init() != OK) + if (SPI::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(accel_report)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } /* perform soft reset (p48) */ write_reg(ADDR_RESET, SOFT_RESET); @@ -308,9 +311,9 @@ BMA180::init() /* disable writing to chip config */ modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); - if (set_range(4)) warnx("Failed setting range"); + if (set_range(4)) { warnx("Failed setting range"); } - if (set_lowpass(75)) warnx("Failed setting lowpass"); + if (set_lowpass(75)) { warnx("Failed setting lowpass"); } if (read_reg(ADDR_CHIP_ID) == CHIP_ID) { ret = OK; @@ -342,8 +345,9 @@ BMA180::probe() /* dummy read to ensure SPI state machine is sane */ read_reg(ADDR_CHIP_ID); - if (read_reg(ADDR_CHIP_ID) == CHIP_ID) + if (read_reg(ADDR_CHIP_ID) == CHIP_ID) { return OK; + } return -EIO; } @@ -356,8 +360,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_interval > 0) { @@ -383,8 +388,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_reports->get(arp)) + if (_reports->get(arp)) { ret = sizeof(*arp); + } return ret; } @@ -397,27 +403,27 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* With internal low pass filters enabled, 250 Hz is sufficient */ return ioctl(filp, SENSORIOCSPOLLRATE, 250); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); @@ -426,16 +432,18 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -443,25 +451,29 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); - return -ENOMEM; + + return OK; } - irqrestore(flags); - - return OK; - } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -543,11 +555,13 @@ BMA180::set_range(unsigned max_g) { uint8_t rangebits; - if (max_g == 0) + if (max_g == 0) { max_g = 16; + } - if (max_g > 16) + if (max_g > 16) { return -ERANGE; + } if (max_g <= 2) { _current_range = 2; @@ -700,7 +714,7 @@ BMA180::measure() * measurement flow without using the external interrupt. */ report.timestamp = hrt_absolute_time(); - report.error_count = 0; + report.error_count = 0; /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. @@ -733,8 +747,9 @@ BMA180::measure() poll_notify(POLLIN); /* publish for subscribers */ - if (_accel_topic != nullptr && !(_pub_blocked)) + if (_accel_topic != nullptr && !(_pub_blocked)) { orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); + } /* stop the perf counter */ perf_end(_sample_perf); @@ -768,26 +783,31 @@ start() { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_ACCEL); - 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(ACCEL_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); fail: @@ -820,14 +840,16 @@ test() ACCEL_DEVICE_PATH); /* reset to manual polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); + } /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); - if (sz != sizeof(a_report)) + if (sz != sizeof(a_report)) { err(1, "immediate acc read failed"); + } warnx("single read"); warnx("time: %lld", a_report.timestamp); @@ -854,14 +876,17 @@ reset() { int fd = open(ACCEL_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); } @@ -872,8 +897,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "BMA180: driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -891,26 +917,30 @@ bma180_main(int argc, char *argv[]) * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { bma180::start(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { bma180::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { bma180::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(argv[1], "info")) { bma180::info(); + } errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); } diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c index 3bdb0d4238b5..83bc2f42347b 100644 --- a/src/drivers/boards/aerocore/aerocore_init.c +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -117,12 +117,12 @@ __END_DECLS static GRAN_HANDLE dma_allocator; -/* - * The DMA heap size constrains the total number of things that can be +/* + * The DMA heap size constrains the total number of things that can be * ready to do DMA at a time. * * For example, FAT DMA depends on one sector-sized buffer per filesystem plus - * one sector-sized buffer per file. + * one sector-sized buffer per file. * * We use a fundamental alignment / granule size of 64B; this is sufficient * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). @@ -137,8 +137,10 @@ dma_alloc_init(void) sizeof(g_dma_heap), 7, /* 128B granule - must be > alignment (XXX bug?) */ 6); /* 64B alignment */ + if (dma_allocator == NULL) { message("[boot] DMA allocator setup FAILED"); + } else { g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); } @@ -262,11 +264,13 @@ __EXPORT int nsh_archinitialize(void) /* Configure Sensors on SPI bus #3 */ spi3 = up_spiinitialize(3); + if (!spi3) { message("[boot] FAILED to initialize SPI port 3\n"); up_ledon(LED_AMBER); return -ENODEV; } + /* Default: 1MHz, 8 bits, Mode 3 */ SPI_SETFREQUENCY(spi3, 10000000); SPI_SETBITS(spi3, 8); @@ -279,11 +283,13 @@ __EXPORT int nsh_archinitialize(void) /* Configure FRAM on SPI bus #4 */ spi4 = up_spiinitialize(4); + if (!spi4) { message("[boot] FAILED to initialize SPI port 4\n"); up_ledon(LED_AMBER); return -ENODEV; } + /* Default: ~10MHz, 8 bits, Mode 3 */ SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000); SPI_SETBITS(spi4, 8); diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c index 22b3a2c240e7..89c636741914 100644 --- a/src/drivers/boards/aerocore/aerocore_led.c +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -103,17 +103,23 @@ __EXPORT void led_toggle(int led) { switch (led) { case 0: - if (stm32_gpioread(GPIO_LED0)) + if (stm32_gpioread(GPIO_LED0)) { stm32_gpiowrite(GPIO_LED0, false); - else + + } else { stm32_gpiowrite(GPIO_LED0, true); + } + break; case 1: - if (stm32_gpioread(GPIO_LED1)) + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } + break; default: diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c index 5a4dd15b26b1..28103c843e4e 100644 --- a/src/drivers/boards/aerocore/aerocore_spi.c +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -67,7 +67,7 @@ * ************************************************************************************/ -__EXPORT void weak_function stm32_spiinitialize(void) +__EXPORT void stm32_spiinitialize(void) { #ifdef CONFIG_STM32_SPI1 stm32_configgpio(GPIO_SPI1_NSS); @@ -136,8 +136,8 @@ __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); break; diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 1ef2e917dd83..155173db5f92 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -82,6 +82,9 @@ __BEGIN_DECLS #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) +/* SPI4--Ramtron */ +#define PX4_SPI_BUS_RAMTRON 4 + /* Nominal chip selects for devices on SPI bus #3 */ #define PX4_SPIDEV_ACCEL_MAG 0 #define PX4_SPIDEV_GYRO 1 @@ -112,6 +115,18 @@ __BEGIN_DECLS #define GPIO_GPIO10_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) #define GPIO_GPIO11_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + */ +#define ADC_CHANNELS (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) + +// ADC defines to be used in sensors.cpp to read from a particular channel +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) +#define ADC_AIRSPEED_VOLTAGE_CHANNEL ((uint8_t)(-1)) + /* PWM * * Eight PWM outputs are configured. diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp deleted file mode 100644 index be514eeb2b60..000000000000 --- a/src/drivers/hil/hil.cpp +++ /dev/null @@ -1,892 +0,0 @@ -/**************************************************************************** - * - * 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 hil.cpp - * - * Driver/configurator for the virtual HIL port. - * - * This virtual driver emulates PWM / servo outputs for setups where - * the connected hardware does not provide enough or no PWM outputs. - * - * Its only function is to take actuator_control uORB messages, - * mix them with any loaded mixer and output the result to the - * actuator_output uORB topic. HIL can also be performed with normal - * PWM outputs, a special flag prevents the outputs to be operated - * during HIL mode. If HIL is not performed with a standalone FMU, - * but in a real system, it is NOT recommended to use this virtual - * driver. Use instead the normal FMU or IO driver. - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include - -#ifdef __PX4_NUTTX -class HIL : public device::CDev -#else -class HIL : public device::VDev -#endif -{ -public: - enum Mode { - MODE_2PWM, - MODE_4PWM, - MODE_8PWM, - MODE_12PWM, - MODE_16PWM, - MODE_NONE - }; - HIL(); - virtual ~HIL(); - - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - - virtual int init(); - - int set_mode(Mode mode); - int set_pwm_rate(unsigned rate); - -private: - static const unsigned _max_actuators = 4; - - Mode _mode; - int _update_rate; - int _current_update_rate; - int _task; - int _t_actuators; - int _t_armed; - orb_advert_t _t_outputs; - unsigned _num_outputs; - bool _primary_pwm_device; - - volatile bool _task_should_exit; - bool _armed; - - MixerGroup *_mixers; - - actuator_controls_s _controls; - - static void task_main_trampoline(int argc, char *argv[]); - void task_main(); - - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); - - int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); - - struct GPIOConfig { - uint32_t input; - uint32_t output; - uint32_t alt; - }; - - static const GPIOConfig _gpio_tab[]; - static const unsigned _ngpio; - - void gpio_reset(void); - void gpio_set_function(uint32_t gpios, int function); - void gpio_write(uint32_t gpios, int function); - uint32_t gpio_read(void); - int gpio_ioctl(device::file_t *filp, int cmd, unsigned long arg); - -}; - -namespace -{ - -HIL *g_hil; - -} // namespace - -HIL::HIL() : -#ifdef __PX4_NUTTX - CDev( -#else - VDev( -#endif - "hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), - _mode(MODE_NONE), - _update_rate(50), - _current_update_rate(0), - _task(-1), - _t_actuators(-1), - _t_armed(-1), - _t_outputs(0), - _num_outputs(0), - _primary_pwm_device(false), - _task_should_exit(false), - _armed(false), - _mixers(nullptr) -{ - _debug_enabled = true; -} - -HIL::~HIL() -{ - if (_task != -1) { - /* tell the task we want it to go away */ - _task_should_exit = true; - - unsigned i = 10; - do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); - - /* if we have given up, kill it */ - if (--i == 0) { - px4_task_delete(_task); - break; - } - - } while (_task != -1); - } - - // XXX already claimed with CDEV - // /* clean up the alternate device node */ - // if (_primary_pwm_device) - // unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - g_hil = nullptr; -} - -int -HIL::init() -{ - int ret; - - ASSERT(_task == -1); - - /* do regular cdev init */ -#ifdef __PX4_NUTTX - ret = CDev::init(); -#else - ret = VDev::init(); -#endif - - if (ret != OK) - return ret; - - // XXX already claimed with CDEV - ///* 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); - if (ret == OK) { - log("default PWM output device"); - _primary_pwm_device = true; - } - - /* reset GPIOs */ - // gpio_reset(); - - /* start the HIL interface task */ - _task = px4_task_spawn_cmd("fmuhil", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1200, - (px4_main_t)&HIL::task_main_trampoline, - nullptr); - - if (_task < 0) { - debug("task start failed: %d", errno); - return -errno; - } - - return OK; -} - -void -HIL::task_main_trampoline(int argc, char *argv[]) -{ - g_hil->task_main(); -} - -int -HIL::set_mode(Mode mode) -{ - /* - * Configure for PWM output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_2PWM: - debug("MODE_2PWM"); - /* multi-port with flow control lines as PWM */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_4PWM: - debug("MODE_4PWM"); - /* multi-port as 4 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_8PWM: - debug("MODE_8PWM"); - /* multi-port as 8 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_12PWM: - debug("MODE_12PWM"); - /* multi-port as 12 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_16PWM: - debug("MODE_16PWM"); - /* multi-port as 16 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_NONE: - debug("MODE_NONE"); - /* disable servo outputs and set a very low update rate */ - _update_rate = 10; - break; - - default: - return -EINVAL; - } - _mode = mode; - return OK; -} - -int -HIL::set_pwm_rate(unsigned rate) -{ - if ((rate > 500) || (rate < 10)) - return -EINVAL; - - _update_rate = rate; - return OK; -} - -void -HIL::task_main() -{ - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); - /* force a reset of the update rate */ - _current_update_rate = 0; - - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ - - /* advertise the mixed control outputs */ - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - int dummy; - _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), - &outputs, &dummy, ORB_PRIO_LOW); - - px4_pollfd_struct_t fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_armed; - fds[1].events = POLLIN; - - unsigned num_outputs; - - /* select the number of virtual outputs */ - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - - case MODE_4PWM: - num_outputs = 4; - break; - - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: - // XXX only support the lower 8 - trivial to extend - num_outputs = 8; - break; - - case MODE_NONE: - default: - num_outputs = 0; - break; - } - - log("starting"); - - /* loop until killed */ - while (!_task_should_exit) { - - /* handle update rate changes */ - if (_current_update_rate != _update_rate) { - int update_rate_in_ms = int(1000 / _update_rate); - if (update_rate_in_ms < 2) - update_rate_in_ms = 2; - orb_set_interval(_t_actuators, update_rate_in_ms); - // up_pwm_servo_set_rate(_update_rate); - _current_update_rate = _update_rate; - } - - /* sleep waiting for data, but no more than a second */ - int ret = px4_poll(&fds[0], 2, 1000); - - /* this would be bad... */ - if (ret < 0) { - log("poll error %d", errno); - continue; - } - - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - PX4_ISFINITE(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - } else { - /* - * 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] = 900; - } - } - - /* and publish for anyone that cares to see */ - orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs); - } - } - - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; - - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - } - } - - px4_close(_t_actuators); - px4_close(_t_armed); - - /* make sure servos are off */ - // up_pwm_servo_deinit(); - - /* note - someone else is responsible for restoring the GPIO config */ - - /* tell the dtor that we are exiting */ - _task = -1; -} - -int -HIL::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) -{ - const actuator_controls_s *controls = (actuator_controls_s *)handle; - - input = controls->control[control_index]; - return 0; -} - -int -HIL::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - int ret; - - debug("ioctl 0x%04x 0x%08x", cmd, arg); - - // /* try it as a GPIO ioctl first */ - // ret = HIL::gpio_ioctl(filp, cmd, arg); - // if (ret != -ENOTTY) - // return ret; - - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch(_mode) { - case MODE_2PWM: - case MODE_4PWM: - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: - ret = HIL::pwm_ioctl(filp, cmd, arg); - break; - default: - ret = -ENOTTY; - debug("not in a PWM mode"); - break; - } - - /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) { -#ifdef __PX4_NUTTX - ret = CDev::ioctl(filp, cmd, arg); -#else - ret = VDev::ioctl(filp, cmd, arg); -#endif - } - - return ret; -} - -int -HIL::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - int ret = OK; - // int channel; - - lock(); - - switch (cmd) { - case PWM_SERVO_ARM: - // up_pwm_servo_arm(true); - break; - - case PWM_SERVO_DISARM: - // up_pwm_servo_arm(false); - break; - - case PWM_SERVO_SET_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate - g_hil->set_pwm_rate(arg); - break; - - case PWM_SERVO_SET_SELECT_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate - break; - - case PWM_SERVO_SET(2): - case PWM_SERVO_SET(3): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_SET(0): - case PWM_SERVO_SET(1): - if (arg < 2100) { - // channel = cmd - PWM_SERVO_SET(0); -// up_pwm_servo_set(channel, arg); XXX - - } else { - ret = -EINVAL; - } - - break; - - case PWM_SERVO_GET(2): - case PWM_SERVO_GET(3): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_GET(0): - case PWM_SERVO_GET(1): { - // channel = cmd - PWM_SERVO_SET(0); - // *(servo_position_t *)arg = up_pwm_servo_get(channel); - break; - } - - case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - // no restrictions on output grouping - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - - *(uint32_t *)arg = (1 << channel); - break; - } - - case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_4PWM) { - *(unsigned *)arg = 4; - - } else { - *(unsigned *)arg = 2; - } - - break; - - case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - - break; - - case MIXERIOCADDSIMPLE: { - mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - - SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); - - if (mixer->check()) { - delete mixer; - ret = -EINVAL; - - } else { - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); - - _mixers->add_mixer(mixer); - } - - break; - } - - case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - unsigned buflen = strnlen(buf, 1024); - - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - - if (_mixers == nullptr) { - ret = -ENOMEM; - - } else { - - ret = _mixers->load_from_buf(buf, buflen); - - if (ret != 0) { - debug("mixer load failed with %d", ret); - delete _mixers; - _mixers = nullptr; - ret = -EINVAL; - } - } - break; - } - - - default: - ret = -ENOTTY; - break; - } - - unlock(); - - return ret; -} - -namespace -{ - -enum PortMode { - PORT_MODE_UNDEFINED = 0, - PORT1_MODE_UNSET, - PORT1_FULL_PWM, - PORT1_PWM_AND_SERIAL, - PORT1_PWM_AND_GPIO, - PORT2_MODE_UNSET, - PORT2_8PWM, - PORT2_12PWM, - PORT2_16PWM, -}; - -PortMode g_port_mode; - -int -hil_new_mode(PortMode new_mode) -{ - // uint32_t gpio_bits; - - -// /* reset to all-inputs */ -// g_hil->ioctl(0, GPIO_RESET, 0); - - // gpio_bits = 0; - - HIL::Mode servo_mode = HIL::MODE_NONE; - - switch (new_mode) { - case PORT_MODE_UNDEFINED: - case PORT1_MODE_UNSET: - case PORT2_MODE_UNSET: - /* nothing more to do here */ - break; - - case PORT1_FULL_PWM: - /* select 4-pin PWM mode */ - servo_mode = HIL::MODE_4PWM; - break; - - case PORT1_PWM_AND_SERIAL: - /* select 2-pin PWM mode */ - servo_mode = HIL::MODE_2PWM; -// /* set RX/TX multi-GPIOs to serial mode */ -// gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT1_PWM_AND_GPIO: - /* select 2-pin PWM mode */ - servo_mode = HIL::MODE_2PWM; - break; - - case PORT2_8PWM: - /* select 8-pin PWM mode */ - servo_mode = HIL::MODE_8PWM; - break; - - case PORT2_12PWM: - /* select 12-pin PWM mode */ - servo_mode = HIL::MODE_12PWM; - break; - - case PORT2_16PWM: - /* select 16-pin PWM mode */ - servo_mode = HIL::MODE_16PWM; - break; - } - -// /* adjust GPIO config for serial mode(s) */ -// if (gpio_bits != 0) -// g_hil->ioctl(0, GPIO_SET_ALT_1, gpio_bits); - - /* (re)set the PWM output mode */ - g_hil->set_mode(servo_mode); - - return OK; -} - -int -hil_start(void) -{ - int ret = OK; - - if (g_hil == nullptr) { - - g_hil = new HIL; - - if (g_hil == nullptr) { - ret = -ENOMEM; - - } else { - ret = g_hil->init(); - - if (ret != OK) { - delete g_hil; - g_hil = nullptr; - } - } - } - - return ret; -} - -int -test(void) -{ - int fd; - - fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); - - if (fd < 0) { - puts("open fail"); - return -ENODEV; - } - - px4_ioctl(fd, PWM_SERVO_ARM, 0); - px4_ioctl(fd, PWM_SERVO_SET(0), 1000); - - px4_close(fd); - - return OK; -} - -int -fake(int argc, char *argv[]) -{ - if (argc < 5) { - puts("hil fake (values -100 .. 100)"); - return -EINVAL; - } - - actuator_controls_s ac; - - ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; - - ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; - - ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; - - ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; - - orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - - if (handle == nullptr) { - puts("advertise failed"); - return 1; - } - - return 0; -} - -} // namespace - -extern "C" __EXPORT int hil_main(int argc, char *argv[]); - -static void -usage() { - fprintf(stderr, "HIL: unrecognized command, try:\n"); - fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); -} - -int -hil_main(int argc, char *argv[]) -{ - PortMode new_mode = PORT_MODE_UNDEFINED; - const char *verb; - int ret = OK; - - if (hil_start() != OK) { - warnx("failed to start the HIL driver"); - return 1; - } - - if (argc < 2) { - usage(); - return -EINVAL; - } - verb = argv[1]; - - /* - * Mode switches. - */ - - // this was all cut-and-pasted from the FMU driver; it's junk - if (!strcmp(verb, "mode_pwm")) { - new_mode = PORT1_FULL_PWM; - - } else if (!strcmp(verb, "mode_pwm_serial")) { - new_mode = PORT1_PWM_AND_SERIAL; - - } else if (!strcmp(verb, "mode_pwm_gpio")) { - new_mode = PORT1_PWM_AND_GPIO; - - } else if (!strcmp(verb, "mode_port2_pwm8")) { - new_mode = PORT2_8PWM; - - } else if (!strcmp(verb, "mode_port2_pwm12")) { - new_mode = PORT2_8PWM; - - } else if (!strcmp(verb, "mode_port2_pwm16")) { - new_mode = PORT2_8PWM; - } - - /* was a new mode set? */ - if (new_mode != PORT_MODE_UNDEFINED) { - - /* yes but it's the same mode */ - if (new_mode == g_port_mode) - return OK; - - /* switch modes */ - return hil_new_mode(new_mode); - } - - if (!strcmp(verb, "test")) { - ret = test(); - } - - else if (!strcmp(verb, "fake")) { - ret = fake(argc - 1, argv + 1); - } - - else { - usage(); - ret = -EINVAL; - } - return ret; -} diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk deleted file mode 100644 index 77213b4be7aa..000000000000 --- a/src/drivers/hil/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Hardware in the Loop (HIL) simulation actuator output bank -# - -MODULE_COMMAND = hil - -SRCS = hil.cpp -MAXOPTIMIZATION = -Os - From 2a3578afd8c56e7e66d58a3a32eed0d051e98860 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:11:32 +1100 Subject: [PATCH 249/293] examples: update from upstream --- src/examples/fixedwing_control/main.c | 10 +++++----- src/examples/hwtest/hwtest.c | 6 ++++-- src/examples/matlab_csv_serial/matlab_csv_serial.c | 10 +++++----- src/examples/publisher/publisher_start_nuttx.cpp | 10 +++++----- src/examples/px4_daemon_app/px4_daemon_app.c | 12 ++++++------ src/examples/px4_mavlink_debug/px4_mavlink_debug.c | 5 +++-- src/examples/px4_simple_app/px4_simple_app.c | 5 +++-- src/examples/rover_steering_control/main.cpp | 10 +++++----- src/examples/subscriber/subscriber_start_nuttx.cpp | 10 +++++----- 9 files changed, 41 insertions(+), 37 deletions(-) diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index d6e78fd8216d..3d6aa882696f 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -432,11 +432,11 @@ int ex_fixedwing_control_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd("ex_fixedwing_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_control_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + fixedwing_control_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index a403f2fa24c5..97a525a8e949 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -57,8 +57,10 @@ __EXPORT int ex_hwtest_main(int argc, char *argv[]); int ex_hwtest_main(int argc, char *argv[]) { - warnx("DO NOT FORGET TO STOP THE COMMANDER APP!"); - warnx("(run to do so)"); + warnx("DO NOT FORGET TO STOP THE DEFAULT CONTROL APPS!"); + warnx("(run ,)"); + warnx("( and)"); + warnx("( to do so)"); warnx("usage: http://px4.io/dev/examples/write_output"); struct actuator_controls_s actuators; diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 139a182b228f..b949400e4744 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -104,11 +104,11 @@ int matlab_csv_serial_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = px4_task_spawn_cmd("matlab_csv_serial", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - matlab_csv_serial_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + matlab_csv_serial_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index 2379d7aa6809..b07207e8a55f 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -69,11 +69,11 @@ int publisher_main(int argc, char *argv[]) task_should_exit = false; daemon_task = px4_task_spawn_cmd("publisher", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 7440a07eb403..1495b1122d7e 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -103,11 +103,11 @@ int px4_daemon_app_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = px4_task_spawn_cmd("daemon", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2000, - px4_daemon_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + px4_daemon_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/examples/px4_mavlink_debug/px4_mavlink_debug.c b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c index 2c95a1733060..9d9bf5d0c077 100644 --- a/src/examples/px4_mavlink_debug/px4_mavlink_debug.c +++ b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,8 @@ /** * @file px4_mavlink_debug.c * Debug application example for PX4 autopilot + * + * @author Example User */ #include diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index a1876265da38..ea54574bb9a9 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,8 @@ /** * @file px4_simple_app.c * Minimal application example for PX4 autopilot + * + * @author Example User */ #include diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 887ea8630959..7a227c236658 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -426,11 +426,11 @@ int rover_steering_control_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd("rover_steering_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - rover_steering_control_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + rover_steering_control_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 424e49e9c1fe..16644280be1b 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -69,11 +69,11 @@ int subscriber_main(int argc, char *argv[]) task_should_exit = false; daemon_task = px4_task_spawn_cmd("subscriber", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } From 2af63960a36f5c422711ff158d4e61a4bb0d7d23 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:11:42 +1100 Subject: [PATCH 250/293] ecl: update from upstream --- src/lib/ecl/attitude_fw/ecl_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_controller.h | 2 + .../ecl/attitude_fw/ecl_pitch_controller.cpp | 109 +++++++++--------- .../ecl/attitude_fw/ecl_roll_controller.cpp | 21 ++-- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 32 +++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 9 +- src/lib/ecl/ecl.h | 3 + 7 files changed, 88 insertions(+), 90 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp index 95ccf4130f38..7bf4129e4804 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_controller.cpp @@ -130,7 +130,7 @@ float ECL_Controller::get_desired_bodyrate() float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) { float airspeed_result = airspeed; - if (!isfinite(airspeed)) { + if (!PX4_ISFINITE(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed_result = 0.5f * (minspeed + maxspeed); } else if (airspeed < minspeed) { diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index ac1ac88d04a5..fed7d1d8fbaa 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -75,6 +75,8 @@ struct ECL_ControlData { float airspeed_max; float airspeed; float scaler; + float groundspeed; + float groundspeed_scaler; bool lock_integrator; }; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index c80eb357cfe4..d04fe38ac3fc 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -60,62 +60,24 @@ ECL_PitchController::~ECL_PitchController() float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data) { - float roll = ctl_data.roll; /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.pitch_setpoint) && - isfinite(roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.airspeed))) { + if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && + PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.airspeed))) { perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; } - /* flying inverted (wings upside down) ? */ - bool inverted = false; - - /* roll is used as feedforward term and inverted flight needs to be considered */ - if (fabsf(roll) < math::radians(90.0f)) { - /* not inverted, but numerically still potentially close to infinity */ - roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f)); - - } else { - /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ - - /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ - if (roll > 0.0f) { - /* right hemisphere */ - roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f)); - - } else { - /* left hemisphere */ - roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f)); - } - } - - /* input conditioning */ - float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); - /* calculate the offset in the rate resulting from rolling */ - //xxx needs explanation and conversion to body angular rates or should be removed - float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * - tanf(roll) * sinf(roll)) * _roll_ff; - - if (inverted) { - turn_offset = -turn_offset; - } - /* Calculate the error */ float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = pitch_error / _tc; - /* add turn offset */ - _rate_setpoint += turn_offset; - - /* limit the rate */ //XXX: move to body angluar rates - + /* limit the rate */ if (_max_rate > 0.01f && _max_rate_neg > 0.01f) { if (_rate_setpoint > 0.0f) { _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; @@ -132,14 +94,14 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.pitch_rate) && - isfinite(ctl_data.yaw_rate) && - isfinite(ctl_data.yaw_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && - isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.pitch_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && + PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -160,11 +122,48 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; - /* Transform estimation to body angular rates (jacobian) */ - float pitch_bodyrate = cosf(ctl_data.roll) * ctl_data.pitch_rate + - cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate; + /* apply turning offset to desired bodyrate setpoint*/ + /* flying inverted (wings upside down)*/ + bool inverted = false; + float constrained_roll; + /* roll is used as feedforward term and inverted flight needs to be considered */ + if (fabsf(ctl_data.roll) < math::radians(90.0f)) { + /* not inverted, but numerically still potentially close to infinity */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f)); + + } else { + /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ + inverted = true; + /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ + if (ctl_data.roll > 0.0f) { + /* right hemisphere */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f)); + + } else { + /* left hemisphere */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f)); + } + } + + /* input conditioning */ + float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); + + /* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle. + For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175 + Availible on google books 8/11/2015: + https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/ + float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) * + tanf(constrained_roll) * sinf(constrained_roll))); + + if (inverted) { + body_fixed_turn_offset = -body_fixed_turn_offset; + } + + /* Finally add the turn offset to your bodyrate setpoint*/ + _bodyrate_setpoint += body_fixed_turn_offset; + - _rate_error = _bodyrate_setpoint - pitch_bodyrate; + _rate_error = _bodyrate_setpoint - ctl_data.pitch_rate; if (!lock_integrator && _k_i > 0.0f) { diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 1c43e7ebf7f7..9d40c73f3b40 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -59,7 +59,7 @@ ECL_RollController::~ECL_RollController() float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll_setpoint) && isfinite(ctl_data.roll))) { + if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) { perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -83,13 +83,13 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.pitch) && - isfinite(ctl_data.roll_rate) && - isfinite(ctl_data.yaw_rate) && - isfinite(ctl_data.yaw_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && - isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.roll_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && + PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -109,11 +109,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat /* Transform setpoint to body angular rates (jacobian) */ _bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; - /* Transform estimation to body angular rates (jacobian) */ - float roll_bodyrate = ctl_data.roll_rate - sinf(ctl_data.pitch) * ctl_data.yaw_rate; - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error + _rate_error = _bodyrate_setpoint - ctl_data.roll_rate; //body angular rate error if (!lock_integrator && _k_i > 0.0f) { diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 4dd409283a96..4849bdad79a3 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -101,13 +101,13 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.speed_body_u) && - isfinite(ctl_data.speed_body_v) && - isfinite(ctl_data.speed_body_w) && - isfinite(ctl_data.roll_rate_setpoint) && - isfinite(ctl_data.pitch_rate_setpoint))) { + if (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.speed_body_u) && + PX4_ISFINITE(ctl_data.speed_body_v) && + PX4_ISFINITE(ctl_data.speed_body_w) && + PX4_ISFINITE(ctl_data.roll_rate_setpoint) && + PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) { perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -145,7 +145,7 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control // counter++; - if (!isfinite(_rate_setpoint)) { + if (!PX4_ISFINITE(_rate_setpoint)) { warnx("yaw rate sepoint not finite"); _rate_setpoint = 0.0f; } @@ -156,10 +156,10 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) && - isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -179,7 +179,7 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl /* input conditioning */ float airspeed = ctl_data.airspeed; - if (!isfinite(airspeed)) { + if (!PX4_ISFINITE(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); @@ -198,12 +198,8 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl _bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch))); } - /* Transform estimation to body angular rates (jacobian) */ - float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate + - cosf(ctl_data.roll) * cosf(ctl_data.pitch) * ctl_data.yaw_rate; - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error + _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 81d821054977..511b5fd36f0f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -76,14 +76,15 @@ class __EXPORT ECL_YawController : _coordinated_method = coordinated_method; } -protected: - float _coordinated_min_speed; - enum { COORD_METHOD_OPEN = 0, - COORD_METHOD_CLOSEACC = 1, + COORD_METHOD_CLOSEACC = 1 }; +protected: + float _coordinated_min_speed; + float _max_rate; + int32_t _coordinated_method; float control_bodyrate_impl(const struct ECL_ControlData &ctl_data); diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index aa3c5000a458..39e1f508093a 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,6 +38,9 @@ */ #include +#include #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time +#define ECL_WARN PX4_WARN +#define ECL_INFO PX4_INFO From 34223567643ec9fe5d6d0082b73052f16eec5163 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:12:03 +1100 Subject: [PATCH 251/293] lib: update from upstream --- src/lib/conversion/rotation.cpp | 329 ++++++++++-------- src/lib/external_lgpl/tecs/tecs.cpp | 250 +++++++------ src/lib/external_lgpl/tecs/tecs.h | 56 ++- src/lib/geo/geo.c | 102 ++++-- src/lib/geo/geo.h | 70 +++- src/lib/geo_lookup/geo_mag_declination.c | 26 +- .../launchdetection/CatapultLaunchMethod.cpp | 22 +- src/lib/launchdetection/LaunchDetector.cpp | 18 +- .../launchdetection/launchdetection_params.c | 4 - src/lib/mathlib/math/Matrix.hpp | 59 ++-- src/lib/mathlib/math/Vector.hpp | 27 +- .../mathlib/math/filter/LowPassFilter2p.cpp | 4 + src/lib/mathlib/math/test/test.cpp | 40 +++ src/lib/mathlib/math/test/test.hpp | 9 + 14 files changed, 653 insertions(+), 363 deletions(-) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index ca82ddd5e8ad..f56513e20909 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -55,154 +55,183 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix) __EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z) { - float tmp; - switch (rot) { - case ROTATION_NONE: - case ROTATION_MAX: - return; - case ROTATION_YAW_45: { - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_YAW_90: { - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_YAW_135: { - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_YAW_180: - x = -x; y = -y; - return; - case ROTATION_YAW_225: { - tmp = HALF_SQRT_2*(y - x); - y = -HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_YAW_270: { - tmp = x; x = y; y = -tmp; - return; - } - case ROTATION_YAW_315: { - tmp = HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(y - x); - x = tmp; - return; - } - case ROTATION_ROLL_180: { - y = -y; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_45: { - tmp = HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_90: { - tmp = x; x = y; y = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_135: { - tmp = HALF_SQRT_2*(y - x); - y = HALF_SQRT_2*(y + x); - x = tmp; z = -z; - return; - } - case ROTATION_PITCH_180: { - x = -x; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_225: { - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(y - x); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_270: { - tmp = x; x = -y; y = -tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_315: { - tmp = HALF_SQRT_2*(x - y); - y = -HALF_SQRT_2*(x + y); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_90: { - tmp = z; z = y; y = -tmp; - return; - } - case ROTATION_ROLL_90_YAW_45: { - tmp = z; z = y; y = -tmp; - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_ROLL_90_YAW_90: { - tmp = z; z = y; y = -tmp; - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_ROLL_90_YAW_135: { - tmp = z; z = y; y = -tmp; - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_ROLL_270: { - tmp = z; z = -y; y = tmp; - return; - } - case ROTATION_ROLL_270_YAW_45: { - tmp = z; z = -y; y = tmp; - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_ROLL_270_YAW_90: { - tmp = z; z = -y; y = tmp; - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_ROLL_270_YAW_135: { - tmp = z; z = -y; y = tmp; - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_ROLL_270_YAW_270: { - tmp = z; z = -y; y = tmp; - tmp = x; x = y; y = -tmp; - return; - } - case ROTATION_PITCH_90: { - tmp = z; z = -x; x = tmp; - return; - } - case ROTATION_PITCH_270: { - tmp = z; z = x; x = -tmp; - return; - } - case ROTATION_ROLL_180_PITCH_270: { - tmp = z; z = x; x = tmp; - y = -y; - return; - } - case ROTATION_PITCH_90_YAW_180: { - tmp = x; x = z; z = tmp; - y = -y; - return; - } - } + float tmp; + + switch (rot) { + case ROTATION_NONE: + case ROTATION_MAX: + return; + + case ROTATION_YAW_45: { + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_YAW_90: { + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_YAW_135: { + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_YAW_180: + x = -x; y = -y; + return; + + case ROTATION_YAW_225: { + tmp = HALF_SQRT_2 * (y - x); + y = -HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_YAW_270: { + tmp = x; x = y; y = -tmp; + return; + } + + case ROTATION_YAW_315: { + tmp = HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (y - x); + x = tmp; + return; + } + + case ROTATION_ROLL_180: { + y = -y; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_45: { + tmp = HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_90: { + tmp = x; x = y; y = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_135: { + tmp = HALF_SQRT_2 * (y - x); + y = HALF_SQRT_2 * (y + x); + x = tmp; z = -z; + return; + } + + case ROTATION_PITCH_180: { + x = -x; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_225: { + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (y - x); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_270: { + tmp = x; x = -y; y = -tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_315: { + tmp = HALF_SQRT_2 * (x - y); + y = -HALF_SQRT_2 * (x + y); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_90: { + tmp = z; z = y; y = -tmp; + return; + } + + case ROTATION_ROLL_90_YAW_45: { + tmp = z; z = y; y = -tmp; + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_ROLL_90_YAW_90: { + tmp = z; z = y; y = -tmp; + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_ROLL_90_YAW_135: { + tmp = z; z = y; y = -tmp; + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_ROLL_270: { + tmp = z; z = -y; y = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_45: { + tmp = z; z = -y; y = tmp; + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_90: { + tmp = z; z = -y; y = tmp; + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_135: { + tmp = z; z = -y; y = tmp; + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_270: { + tmp = z; z = -y; y = tmp; + tmp = x; x = y; y = -tmp; + return; + } + + case ROTATION_PITCH_90: { + tmp = z; z = -x; x = tmp; + return; + } + + case ROTATION_PITCH_270: { + tmp = z; z = x; x = -tmp; + return; + } + + case ROTATION_ROLL_180_PITCH_270: { + tmp = z; z = x; x = tmp; + y = -y; + return; + } + + case ROTATION_PITCH_90_YAW_180: { + tmp = x; x = z; z = tmp; + y = -y; + return; + } + } } diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index cfcc48b62a01..cca01e6a3ab1 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -27,7 +27,8 @@ using namespace math; * */ -void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth) +void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, + const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air) { // Implement third order complementary filter for height and height rate // estimted height rate = _integ2_state @@ -45,17 +46,32 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3 // DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2), // accel_earth(0), accel_earth(1), accel_earth(2)); - if (DT > 1.0f) { + bool reset_altitude = false; + + if (_update_50hz_last_usec == 0 || DT > DT_MAX) { + DT = DT_DEFAULT; // when first starting TECS, use small time constant + reset_altitude = true; + } + + if (!altitude_lock || !in_air) { + reset_altitude = true; + } + + if (reset_altitude) { _integ3_state = baro_altitude; _integ2_state = 0.0f; _integ1_state = 0.0f; - DT = 0.02f; // when first starting TECS, use a - // small time constant + + // Reset the filter state as we just switched from non-altitude control + // to altitude control mode + _states_initalized = false; } _update_50hz_last_usec = now; _EAS = airspeed; + _in_air = in_air; + // Get height acceleration float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G); // Perform filter calculation using backwards Euler integration @@ -70,7 +86,7 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3 // If more than 1 second has elapsed since last update then reset the integrator state // to the measured height - if (DT > 1.0f) { + if (reset_altitude) { _integ3_state = baro_altitude; } else { @@ -81,7 +97,7 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3 // Only required if airspeed is being measured and controlled float temp = 0; - if (isfinite(airspeed) && airspeed_sensor_enabled()) { + if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) { // Get DCM // Calculate speed rate of change // XXX check @@ -89,12 +105,16 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3 // take 5 point moving average //_vel_dot = _vdot_filter.apply(temp); // XXX resolve this properly - _vel_dot = 0.9f * _vel_dot + 0.1f * temp; + _vel_dot = 0.95f * _vel_dot + 0.05f * temp; } else { _vel_dot = 0.0f; } + if (!_in_air) { + _states_initalized = false; + } + } void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, @@ -103,7 +123,6 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, // Calculate time in seconds since last update uint64_t now = ecl_absolute_time(); float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f; - _update_speed_last_usec = now; // Convert equivalent airspeeds to true airspeeds @@ -112,17 +131,9 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, _TASmax = indicated_airspeed_max * EAS2TAS; _TASmin = indicated_airspeed_min * EAS2TAS; - // Reset states of time since last update is too large - if (DT > 1.0f) { - _integ5_state = (_EAS * EAS2TAS); - _integ4_state = 0.0f; - DT = 0.1f; // when first starting TECS, use a - // small time constant - } - // Get airspeed or default to halfway between min and max if // airspeed is not being used and set speed rate to zero - if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) { + if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) { // If no airspeed available use average of min and max _EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max); @@ -130,6 +141,16 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, _EAS = indicated_airspeed; } + // Reset states on initial execution or if not active + if (_update_speed_last_usec == 0 || !_in_air) { + _integ4_state = 0.0f; + _integ5_state = (_EAS * EAS2TAS); + } + + if (DT < DT_MIN || DT > DT_MAX) { + DT = DT_DEFAULT; // when first starting TECS, use small time constant + } + // Implement a second order complementary filter to obtain a // smoothed airspeed estimate // airspeed estimate is held in _integ5_state @@ -146,7 +167,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, _integ5_state = _integ5_state + integ5_input * DT; // limit the airspeed to a minimum of 3 m/s _integ5_state = max(_integ5_state, 3.0f); - + _update_speed_last_usec = now; } void TECS::_update_speed_demand(void) @@ -165,40 +186,38 @@ void TECS::_update_speed_demand(void) // calculate velocity rate limits based on physical performance limits // provision to use a different rate limit if bad descent or underspeed condition exists - // Use 50% of maximum energy rate to allow margin for total energy contgroller -// float velRateMax; -// float velRateMin; -// -// if ((_badDescent) || (_underspeed)) { -// velRateMax = 0.5f * _STEdot_max / _integ5_state; -// velRateMin = 0.5f * _STEdot_min / _integ5_state; -// -// } else { -// velRateMax = 0.5f * _STEdot_max / _integ5_state; -// velRateMin = 0.5f * _STEdot_min / _integ5_state; -// } -// + // Use 50% of maximum energy rate to allow margin for total energy controller + float velRateMax; + float velRateMin; + + if ((_badDescent) || (_underspeed)) { + velRateMax = 0.5f * _STEdot_max / _integ5_state; + velRateMin = 0.5f * _STEdot_min / _integ5_state; + + } else { + velRateMax = 0.5f * _STEdot_max / _integ5_state; + velRateMin = 0.5f * _STEdot_min / _integ5_state; + } + // // Apply rate limit -// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { -// _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; +// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * _DT)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMax * _DT; // _TAS_rate_dem = velRateMax; // -// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { -// _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; +// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * _DT)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMin * _DT; // _TAS_rate_dem = velRateMin; // // } else { // _TAS_dem_adj = _TAS_dem; // // -// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; +// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / _DT; // } - _TAS_dem_adj = _TAS_dem; - _TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now + _TAS_dem_adj = constrain(_TAS_dem, _TASmin, _TASmax);; + _TAS_rate_dem = constrain((_TAS_dem_adj - _integ5_state) * _speedrate_p, velRateMin, velRateMax); //xxx: using a p loop for now - // Constrain speed demand again to protect against bad values on initialisation. - _TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax); // _TAS_dem_last = _TAS_dem; // warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f", @@ -209,23 +228,29 @@ void TECS::_update_speed_demand(void) void TECS::_update_height_demand(float demand, float state) { -// // Apply 2 point moving average to demanded height -// // This is required because height demand is only updated at 5Hz -// _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); -// _hgt_dem_in_old = _hgt_dem; -// -// // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, -// // _maxClimbRate); -// -// // Limit height rate of change -// if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { -// _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; -// -// } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { -// _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; -// } -// -// _hgt_dem_prev = _hgt_dem; + // Handle initialization + if (PX4_ISFINITE(demand) && fabsf(_hgt_dem_in_old) < 0.1f) { + _hgt_dem_in_old = demand; + } + // Apply 2 point moving average to demanded height + // This is required because height demand is updated in steps + if (PX4_ISFINITE(demand)) { + _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); + } else { + _hgt_dem = _hgt_dem_in_old; + } + _hgt_dem_in_old = _hgt_dem; + + // Limit height demand + // this is important to avoid a windup + if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * _DT)) { + _hgt_dem = _hgt_dem_prev + _maxClimbRate * _DT; + + } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * _DT)) { + _hgt_dem = _hgt_dem_prev - _maxSinkRate * _DT; + } + + _hgt_dem_prev = _hgt_dem; // // // Apply first order lag to height demand // _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; @@ -235,9 +260,9 @@ void TECS::_update_height_demand(float demand, float state) // // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, // // _hgt_rate_dem); - _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; - _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; + _hgt_dem_adj = 0.1f * _hgt_dem + 0.9f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; + _hgt_rate_dem = (_hgt_dem_adj - state) * _heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last) / _DT; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { @@ -289,12 +314,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); - float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; + _STEdot_error = STEdot_dem - _SPEdot - _SKEdot; // Apply 0.5 second first order filter to STEdot_error // This is required to remove accelerometer noise from the measurement - STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast; - _STEdotErrLast = STEdot_error; + _STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast; + _STEdotErrLast = _STEdot_error; // Calculate throttle demand // If underspeed condition is set, then demand full throttle @@ -322,7 +347,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } // 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 = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); // Rate limit PD + FF throttle @@ -416,13 +441,13 @@ void TECS::_update_pitch(void) float SPE_weighting = 2.0f - SKE_weighting; // Calculate Specific Energy Balance demand, and error - float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; - float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; - float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); - float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); + float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; + float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; + _SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); + _SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); // Calculate integrator state, constraining input if pitch limits are exceeded - float integ7_input = SEB_error * _integGain; + float integ7_input = _SEB_error * _integGain; if (_pitch_dem_unc > _PITCHmaxf) { integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc); @@ -440,7 +465,7 @@ void TECS::_update_pitch(void) // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the // integrator has to catch up before the nose can be raised to reduce speed during climbout. float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); - float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; + float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; if (_climbOutDem) { temp += _PITCHminf * gainInv; @@ -471,21 +496,28 @@ void TECS::_update_pitch(void) void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad) { // Initialise states and variables if DT > 1 second or in climbout - if (_DT > 1.0f) { - _integ6_state = 0.0f; - _integ7_state = 0.0f; + if (_update_pitch_throttle_last_usec == 0 || _DT > DT_MAX || !_in_air || !_states_initalized) { + _integ1_state = 0.0f; + _integ2_state = 0.0f; + _integ3_state = baro_altitude; + _integ4_state = 0.0f; + _integ5_state = _EAS; + _integ6_state = 0.0f; + _integ7_state = 0.0f; _last_throttle_dem = throttle_cruise; - _last_pitch_dem = pitch; - _hgt_dem_adj_last = baro_altitude; - _hgt_dem_adj = _hgt_dem_adj_last; - _hgt_dem_prev = _hgt_dem_adj_last; - _hgt_dem_in_old = _hgt_dem_adj_last; - _TAS_dem_last = _TAS_dem; - _TAS_dem_adj = _TAS_dem; - _underspeed = false; - _badDescent = false; - _DT = 0.1f; // when first starting TECS, use a - // small time constant + _last_pitch_dem = pitch; + _hgt_dem_adj_last = baro_altitude; + _hgt_dem_adj = _hgt_dem_adj_last; + _hgt_dem_prev = _hgt_dem_adj_last; + _hgt_dem_in_old = _hgt_dem_adj_last; + _TAS_dem_last = _TAS_dem; + _TAS_dem_adj = _TAS_dem; + _underspeed = false; + _badDescent = false; + + if (_DT > DT_MAX || _DT < DT_MIN) { + _DT = DT_DEFAULT; + } } else if (_climbOutDem) { _PITCHminf = ptchMinCO_rad; @@ -498,6 +530,8 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt _underspeed = false; _badDescent = false; } + + _states_initalized = true; } void TECS::_update_STE_rate_lim(void) @@ -512,20 +546,17 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { + // Calculate time in seconds since last update uint64_t now = ecl_absolute_time(); _DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f; - _update_pitch_throttle_last_usec = now; // printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n", // _DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max); - // Update the speed estimate using a 2nd order complementary filter - _update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS); - // Convert inputs - _THRmaxf = throttle_max; - _THRminf = throttle_min; + _THRmaxf = throttle_max; + _THRminf = throttle_min; _PITCHmaxf = pitch_limit_max; _PITCHminf = pitch_limit_min; _climbOutDem = climbOutDem; @@ -533,6 +564,13 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // initialise selected states and variables if DT > 1 second or in climbout _initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO); + if (!_in_air) { + return; + } + + // Update the speed estimate using a 2nd order complementary filter + _update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS); + // Calculate Specific Total Energy Rate Limits _update_STE_rate_lim(); @@ -570,17 +608,27 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f _tecs_state.mode = ECL_TECS_MODE_NORMAL; } - _tecs_state.hgt_dem = _hgt_dem_adj; - _tecs_state.hgt = _integ3_state; - _tecs_state.dhgt_dem = _hgt_rate_dem; - _tecs_state.dhgt = _integ2_state; - _tecs_state.spd_dem = _TAS_dem_adj; - _tecs_state.spd = _integ5_state; - _tecs_state.dspd = _vel_dot; - _tecs_state.ithr = _integ6_state; - _tecs_state.iptch = _integ7_state; - _tecs_state.thr = _throttle_dem; - _tecs_state.ptch = _pitch_dem; - _tecs_state.dspd_dem = _TAS_rate_dem; + _tecs_state.altitude_sp = _hgt_dem_adj; + _tecs_state.altitude_filtered = _integ3_state; + _tecs_state.altitude_rate_sp = _hgt_rate_dem; + _tecs_state.altitude_rate = _integ2_state; + + _tecs_state.airspeed_sp = _TAS_dem_adj; + _tecs_state.airspeed_rate_sp = _TAS_rate_dem; + _tecs_state.airspeed_filtered = _integ5_state; + _tecs_state.airspeed_rate = _vel_dot; + + _tecs_state.total_energy_error = _STE_error; + _tecs_state.energy_distribution_error = _SEB_error; + _tecs_state.total_energy_rate_error = _STEdot_error; + _tecs_state.energy_distribution_error = _SEBdot_error; + + _tecs_state.energy_error_integ = _integ6_state; + _tecs_state.energy_distribution_error_integ = _integ7_state; + + _tecs_state.throttle_integ = _integ6_state; + _tecs_state.pitch_integ = _integ7_state; + + _update_pitch_throttle_last_usec = now; } diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index eb45237b7de8..1ca742884973 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -60,6 +60,7 @@ class __EXPORT TECS _integ7_state(0.0f), _last_pitch_dem(0.0f), _vel_dot(0.0f), + _EAS(0.0f), _TAS_dem(0.0f), _TAS_dem_last(0.0f), _hgt_dem_in_old(0.0f), @@ -79,7 +80,13 @@ class __EXPORT TECS _SKE_est(0.0f), _SPEdot(0.0f), _SKEdot(0.0f), + _STE_error(0.0f), + _STEdot_error(0.0f), + _SEB_error(0.0f), + _SEBdot_error(0.0f), _airspeed_enabled(false), + _states_initalized(false), + _in_air(false), _throttle_slewrate(0.0f) { } @@ -95,7 +102,8 @@ class __EXPORT TECS // Update of the estimated height and height rate internal state // Update of the inertial speed rate internal state // Should be called at 50Hz or greater - void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth); + void update_state(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, + const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air); // Update the control loop calculations void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, @@ -110,6 +118,9 @@ class __EXPORT TECS return get_throttle_demand(); } + void reset_state() { + _states_initalized = false; + } float get_pitch_demand() { return _pitch_dem; } @@ -134,18 +145,22 @@ class __EXPORT TECS struct tecs_state { uint64_t timestamp; - float hgt; - float dhgt; - float hgt_dem; - float dhgt_dem; - float spd_dem; - float spd; - float dspd; - float ithr; - float iptch; - float thr; - float ptch; - float dspd_dem; + float altitude_filtered; + float altitude_sp; + float altitude_rate; + float altitude_rate_sp; + float airspeed_filtered; + float airspeed_sp; + float airspeed_rate; + float airspeed_rate_sp; + float energy_error_integ; + float energy_distribution_error_integ; + float total_energy_error; + float total_energy_rate_error; + float energy_distribution_error; + float energy_distribution_rate_error; + float throttle_integ; + float pitch_integ; enum ECL_TECS_MODE mode; }; @@ -373,10 +388,25 @@ class __EXPORT TECS // Specific energy error quantities float _STE_error; + // Energy error rate + float _STEdot_error; + + // Specific energy balance error + float _SEB_error; + + // Specific energy balance error rate + float _SEBdot_error; + // Time since last update of main TECS loop (seconds) float _DT; + static constexpr float DT_MIN = 0.001f; + static constexpr float DT_DEFAULT = 0.02f; + static constexpr float DT_MAX = 1.0f; + bool _airspeed_enabled; + bool _states_initalized; + bool _in_air; float _throttle_slewrate; float _indicated_airspeed_min; float _indicated_airspeed_max; diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 30b9d9ec2813..af49bf150491 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -83,16 +83,19 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference return ref->timestamp; } -__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_global_init(double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { if (strcmp("commander", getprogname()) == 0) { return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); + } else { return -1; } } -__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { ref->lat_rad = lat_0 * M_DEG_TO_RAD; @@ -106,7 +109,8 @@ __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s * return 0; } -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, + double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); } @@ -116,7 +120,8 @@ __EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lo return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad); } -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad) { if (!map_projection_initialized(ref)) { return -1; @@ -134,7 +139,8 @@ __EXPORT int map_projection_global_project(double lat, double lon, float *x, flo } -__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y) { if (!map_projection_initialized(ref)) { return -1; @@ -161,7 +167,8 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub return map_projection_reproject(&mp_ref, x, y, lat, lon); } -__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon) { if (!map_projection_initialized(ref)) { return -1; @@ -212,14 +219,16 @@ __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, { if (strcmp("commander", getprogname()) == 0) { gl_ref.alt = alt_0; - if (!map_projection_global_init(lat_0, lon_0, timestamp)) - { + + if (!map_projection_global_init(lat_0, lon_0, timestamp)) { gl_ref.init_done = true; return 0; + } else { gl_ref.init_done = false; return -1; } + } else { return -1; } @@ -260,8 +269,7 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al return -1; } - if (map_projection_global_getref(lat_0, lon_0)) - { + if (map_projection_global_getref(lat_0, lon_0)) { return -1; } @@ -283,12 +291,48 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; - double a = sin(d_lat / (double)2.0) * sin(d_lat / (double)2.0) + sin(d_lon / (double)2.0) * sin(d_lon / (double)2.0) * cos(lat_now_rad) * cos(lat_next_rad); + double a = sin(d_lat / (double)2.0) * sin(d_lat / (double)2.0) + sin(d_lon / (double)2.0) * sin(d_lon / + (double)2.0) * cos(lat_now_rad) * cos(lat_next_rad); double c = (double)2.0 * atan2(sqrt(a), sqrt((double)1.0 - a)); return CONSTANTS_RADIUS_OF_EARTH * c; } +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target) +{ + if (fabsf(dist) < FLT_EPSILON) { + *lat_target = lat_A; + *lon_target = lon_A; + + } else if (dist >= FLT_EPSILON) { + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + + } else { + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + heading = _wrap_2pi(heading + M_PI_F); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } +} + +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_target, double *lon_target) +{ + bearing = _wrap_2pi(bearing); + double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); + + double lat_start_rad = lat_start * M_DEG_TO_RAD; + double lon_start_rad = lon_start * M_DEG_TO_RAD; + + *lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos(( + double)bearing)); + *lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad), + cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target)); + + *lat_target *= M_RAD_TO_DEG; + *lon_target *= M_RAD_TO_DEG; +} __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { double lat_now_rad = lat_now * M_DEG_TO_RAD; @@ -299,14 +343,16 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , + cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); theta = _wrap_pi(theta); return theta; } -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, + float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -316,11 +362,13 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + *v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos( + d_lon)); *v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); } -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, + float *v_n, float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -335,7 +383,8 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d *v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad); } -__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res) +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, + double *lon_res) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -346,7 +395,8 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa // Additional functions - @author Doug Weibel -__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_start, double lon_start, double lat_end, double lon_end) { // This function returns the distance to the nearest point on the track line. Distance is positive if current // position is right of the track and negative if left of the track as seen from a point on the track line @@ -397,7 +447,8 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d } -__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, +__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep) { // This function returns the distance to the nearest point on the track arc. Distance is positive if current @@ -436,10 +487,12 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do in_sector = false; // Case where sector does not span zero - if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; } + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start + && bearing_now <= bearing_sector_end) { in_sector = true; } // Case where sector does span zero - if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; } + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start + || bearing_now < bearing_sector_end)) { in_sector = true; } // If in the sector then calculate distance and bearing to closest point if (in_sector) { @@ -479,6 +532,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do if (dist_to_start < dist_to_end) { crosstrack_error->distance = dist_to_start; crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + } else { crosstrack_error->past_end = true; crosstrack_error->distance = dist_to_end; @@ -539,6 +593,7 @@ __EXPORT float _wrap_pi(float bearing) } int c = 0; + while (bearing >= M_PI_F) { bearing -= M_TWOPI_F; @@ -548,6 +603,7 @@ __EXPORT float _wrap_pi(float bearing) } c = 0; + while (bearing < -M_PI_F) { bearing += M_TWOPI_F; @@ -567,6 +623,7 @@ __EXPORT float _wrap_2pi(float bearing) } int c = 0; + while (bearing >= M_TWOPI_F) { bearing -= M_TWOPI_F; @@ -576,6 +633,7 @@ __EXPORT float _wrap_2pi(float bearing) } c = 0; + while (bearing < 0.0f) { bearing += M_TWOPI_F; @@ -595,6 +653,7 @@ __EXPORT float _wrap_180(float bearing) } int c = 0; + while (bearing >= 180.0f) { bearing -= 360.0f; @@ -604,6 +663,7 @@ __EXPORT float _wrap_180(float bearing) } c = 0; + while (bearing < -180.0f) { bearing += 360.0f; @@ -623,6 +683,7 @@ __EXPORT float _wrap_360(float bearing) } int c = 0; + while (bearing >= 360.0f) { bearing -= 360.0f; @@ -632,6 +693,7 @@ __EXPORT float _wrap_360(float bearing) } c = 0; + while (bearing < 0.0f) { bearing += 360.0f; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index ca3587b93496..11451fcf1988 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -113,7 +113,8 @@ __EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lo * Writes the reference values of the projection given by the argument to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad); +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad); /** * Initializes the global map transformation. @@ -158,15 +159,16 @@ __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double __EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y); - /* Transforms a point in the geographic coordinate system to the local - * azimuthal equidistant plane using the projection given by the argument - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - * @return 0 if map_projection_init was called before, -1 else - */ -__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); +/* Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the projection given by the argument +* @param x north +* @param y east +* @param lat in degrees (47.1234567°, not 471234567°) +* @param lon in degrees (8.1234567°, not 81234567°) +* @return 0 if map_projection_init was called before, -1 else +*/ +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y); /** * Transforms a point in the local azimuthal equidistant plane to the @@ -190,7 +192,8 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub * @param lon in degrees (8.1234567°, not 81234567°) * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon); /** * Get reference position of the global map projection @@ -233,6 +236,36 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al */ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +/** + * Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance + * from waypoint A + * + * @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°) + * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°) + * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°) + * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°) + * @param dist distance of target waypoint from waypoint A in meters (can be negative) + * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°) + */ +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target); + +/** + * Creates a waypoint from given waypoint, distance and bearing + * see http://www.movable-type.co.uk/scripts/latlong.html + * + * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°) + * @param bearing in rad + * @param distance in meters + * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°) + */ +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_target, double *lon_target); + /** * Returns the bearing to the next waypoint in radians. * @@ -243,15 +276,20 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e); +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, + float *v_e); -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e); +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, + float *v_n, float *v_e); -__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res); +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, + double *lon_res); -__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); +__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_start, double lon_start, double lat_end, double lon_end); -__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, +__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep); /* diff --git a/src/lib/geo_lookup/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c index c41d52606134..58a3a351d912 100644 --- a/src/lib/geo_lookup/geo_mag_declination.c +++ b/src/lib/geo_lookup/geo_mag_declination.c @@ -35,7 +35,7 @@ * @file geo_mag_declination.c * * Calculation / lookup table for earth magnetic field declination. -* +* * Lookup table from Scott Ferguson * * XXX Lookup table currently too coarse in resolution (only full degrees) @@ -55,18 +55,18 @@ static const int8_t declination_table[13][37] = \ { { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, - { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, - { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, - { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, - { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, - { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, - { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, - { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, - { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, - { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, - { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, - { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, - { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, + { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, + { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, + { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, + { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, + { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, + { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, + { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, + { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, + { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, + { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, + { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, + { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, }; static float get_lookup_table_val(unsigned lat, unsigned lon); diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 2ea1c414bdde..b5692cc7e7c7 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -58,7 +58,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : } -CatapultLaunchMethod::~CatapultLaunchMethod() { +CatapultLaunchMethod::~CatapultLaunchMethod() +{ } @@ -69,34 +70,41 @@ void CatapultLaunchMethod::update(float accel_x) switch (state) { case LAUNCHDETECTION_RES_NONE: + /* Detect a acceleration that is longer and stronger as the minimum given by the params */ if (accel_x > thresholdAccel.get()) { integrator += dt; + if (integrator > thresholdTime.get()) { if (motorDelay.get() > 0.0f) { state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; - warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full" - " throttle", (double)motorDelay.get()); + warnx("Launch detected: enablecontrol, waiting %8.4fs until full throttle", + double(motorDelay.get())); + } else { /* No motor delay set: go directly to enablemotors state */ state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; - warnx("Launch detected: state: enablemotors (delay not activated)"); + warnx("Launch detected: enablemotors (delay not activated)"); } } + } else { /* reset */ reset(); } + break; case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL: - /* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is + /* Vehicle is currently controlling attitude but not with full throttle. Waiting until delay is * over to allow full throttle */ motorDelayCounter += dt; + if (motorDelayCounter > motorDelay.get()) { warnx("Launch detected: state enablemotors"); state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } + break; default: @@ -119,10 +127,12 @@ void CatapultLaunchMethod::reset() state = LAUNCHDETECTION_RES_NONE; } -float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) { +float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) +{ /* If motor is turned on do not impose the extra limit on maximum pitch */ if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { return pitchMaxDefault; + } else { return pitchMaxPreThrottle.get(); } diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 52f5c3257669..38e556e88c9a 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -66,7 +66,7 @@ LaunchDetector::~LaunchDetector() void LaunchDetector::reset() { /* Reset all detectors */ - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) { launchMethods[i]->reset(); } @@ -79,7 +79,7 @@ void LaunchDetector::reset() void LaunchDetector::update(float accel_x) { if (launchdetection_on.get() == 1) { - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) { launchMethods[i]->update(accel_x); } } @@ -89,14 +89,15 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected() { if (launchdetection_on.get() == 1) { if (activeLaunchDetectionMethodIndex < 0) { - /* None of the active launchmethods has detected a launch, check all launchmethods */ - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { + /* None of the active launchmethods has detected a launch, check all launchmethods */ + for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) { + if (launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { warnx("selecting launchmethod %d", i); activeLaunchDetectionMethodIndex = i; // from now on only check this method return launchMethods[i]->getLaunchDetected(); } } + } else { return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected(); } @@ -105,7 +106,8 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected() return LAUNCHDETECTION_RES_NONE; } -float LaunchDetector::getPitchMax(float pitchMaxDefault) { +float LaunchDetector::getPitchMax(float pitchMaxDefault) +{ if (!launchdetection_on.get()) { return pitchMaxDefault; } @@ -113,11 +115,13 @@ float LaunchDetector::getPitchMax(float pitchMaxDefault) { /* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method, * otherwise use the default limit */ if (activeLaunchDetectionMethodIndex < 0) { - if (sizeof(launchMethods)/sizeof(LaunchMethod) > 1) { + if (sizeof(launchMethods) / sizeof(LaunchMethod *) > 1) { return pitchMaxDefault; + } else { return launchMethods[0]->getPitchMax(pitchMaxDefault); } + } else { return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault); } diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 5159f2fcb246..e5070eeaaf67 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * Catapult launch detection parameters, accessible via MAVLink * diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 2a7b61238bfd..54a1b87ee047 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -49,8 +49,7 @@ #ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" #else -#include -#include +#include "matrix/math.hpp" #endif #include @@ -126,15 +125,6 @@ class __EXPORT MatrixBase memcpy(data, d, sizeof(data)); } -#if defined(__PX4_ROS) - /** - * set data from boost::array - */ - void set(const boost::array d) { - set(static_cast(d.data())); - } -#endif - /** * set row from vector */ @@ -294,7 +284,7 @@ class __EXPORT MatrixBase for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] / num; + res.data[i][j] = data[i][j] / num; return res; } @@ -317,11 +307,9 @@ class __EXPORT MatrixBase arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::Matrix Him = Eigen::Map > - (m.arm_mat.pData); - Eigen::Matrix Product = Me * Him; + matrix::Matrix Me(this->arm_mat.pData); + matrix::Matrix Him(m.arm_mat.pData); + matrix::Matrix Product = Me * Him; Matrix res(Product.data()); return res; #endif @@ -336,10 +324,8 @@ class __EXPORT MatrixBase arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Me.transposeInPlace(); - Matrix res(Me.data()); + matrix::Matrix Me(this->arm_mat.pData); + Matrix res(Me.transpose().data()); return res; #endif } @@ -353,10 +339,8 @@ class __EXPORT MatrixBase arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea - Matrix res(MyInverse.data()); + matrix::SquareMatrix Me = matrix::Matrix(this->arm_mat.pData); + Matrix res(Me.I().data()); return res; #endif } @@ -384,7 +368,7 @@ class __EXPORT MatrixBase printf("[ "); for (unsigned int j = 0; j < N; j++) - printf("%.3f\t", data[i][j]); + printf("%.3f\t", (double)data[i][j]); printf(" ]\n"); } @@ -421,11 +405,9 @@ class __EXPORT Matrix : public MatrixBase Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); #else - //probably nicer if this could go into a function like "eigen_mat_mult" or so - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData, N); - Eigen::VectorXf Product = Me * Vec; + matrix::Matrix Me(this->arm_mat.pData); + matrix::Matrix Vec(v.arm_col.pData); + matrix::Matrix Product = Me * Vec; Vector res(Product.data()); #endif return res; @@ -445,6 +427,21 @@ class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> Matrix(const float *d) : MatrixBase<3, 3>(d) {} Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {} + /** + * set data + */ + void set(const float d[9]) { + memcpy(data, d, sizeof(data)); + } + +#if defined(__PX4_ROS) + /** + * set data from boost::array + */ + void set(const boost::array d) { + set(static_cast(d.data())); + } +#endif /** * set to value diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 781c14d53991..00c0411167a9 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -353,7 +353,7 @@ class __EXPORT VectorBase printf("[ "); for (unsigned int i = 0; i < N; i++) - printf("%.3f\t", data[i]); + printf("%.3f\t", (double)data[i]); printf("]\n"); } @@ -407,7 +407,14 @@ class __EXPORT Vector<2> : public VectorBase<2> data[0] = d[0]; data[1] = d[1]; } - +#if defined(__PX4_ROS) + /** + * set data from boost::array + */ + void set(const boost::array d) { + set(static_cast(d.data())); + } +#endif /** * set to value */ @@ -444,6 +451,14 @@ class __EXPORT Vector<3> : public VectorBase<3> data[1] = y; data[2] = z; } +#if defined(__PX4_ROS) + /** + * set data from boost::array + */ + void set(const boost::array d) { + set(static_cast(d.data())); + } +#endif /** * set data @@ -494,6 +509,14 @@ class __EXPORT Vector<4> : public VectorBase<4> data[2] = x2; data[3] = x3; } +#if defined(__PX4_ROS) + /** + * set data from boost::array + */ + void set(const boost::array d) { + set(static_cast(d.data())); + } +#endif /** * set data diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 7e2778e4450f..c3d44588fdfc 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -41,6 +41,10 @@ #include "LowPassFilter2p.hpp" #include "math.h" +#ifndef M_PI_F +#define M_PI_F 3.14159f +#endif + namespace math { diff --git a/src/lib/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp index aff31bca0d91..1588511931ec 100644 --- a/src/lib/mathlib/math/test/test.cpp +++ b/src/lib/mathlib/math/test/test.cpp @@ -54,6 +54,46 @@ bool __EXPORT equal(float a, float b, float epsilon) } else { return true; } } +bool __EXPORT greater_than(float a, float b) +{ + if (a > b) { + return true; + } else { + printf("not a > b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + +bool __EXPORT less_than(float a, float b) +{ + if (a < b) { + return true; + } else { + printf("not a < b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + +bool __EXPORT greater_than_or_equal(float a, float b) +{ + if (a >= b) { + return true; + } else { + printf("not a >= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + +bool __EXPORT less_than_or_equal(float a, float b) +{ + if (a <= b) { + return true; + } else { + printf("not a <= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + void __EXPORT float2SigExp( const float &num, float &sig, diff --git a/src/lib/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp index 2027bb827ca0..48b936b146a7 100644 --- a/src/lib/mathlib/math/test/test.hpp +++ b/src/lib/mathlib/math/test/test.hpp @@ -44,6 +44,15 @@ //#include bool equal(float a, float b, float eps = 1e-5); + +bool greater_than(float a, float b); + +bool less_than(float a, float b); + +bool greater_than_or_equal(float a, float b); + +bool less_than_or_equal(float a, float b); + void float2SigExp( const float &num, float &sig, From ba4e39a4c23f36da1f4f6b972d13527bc9d86418 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:12:16 +1100 Subject: [PATCH 252/293] platforms: update from upstream --- src/platforms/posix/px4_layer/dq_addlast.c | 74 ---- src/platforms/posix/px4_layer/dq_rem.c | 84 ----- src/platforms/posix/px4_layer/dq_remfirst.c | 82 ----- src/platforms/posix/px4_layer/queue.c | 102 ------ src/platforms/posix/px4_layer/sq_addafter.c | 71 ---- src/platforms/posix/px4_layer/sq_addlast.c | 72 ---- src/platforms/posix/px4_layer/sq_remfirst.c | 76 ---- src/platforms/posix/px4_layer/work_cancel.c | 120 ------- src/platforms/posix/px4_layer/work_lock.h | 53 --- src/platforms/posix/px4_layer/work_queue.c | 133 ------- src/platforms/posix/px4_layer/work_thread.c | 328 ------------------ .../qurt/px4_layer/commands_default.c | 68 ---- src/platforms/qurt/px4_layer/dq_addlast.c | 74 ---- src/platforms/qurt/px4_layer/dq_rem.c | 84 ----- src/platforms/qurt/px4_layer/dq_remfirst.c | 82 ----- src/platforms/qurt/px4_layer/queue.c | 102 ------ src/platforms/qurt/px4_layer/sq_addafter.c | 71 ---- src/platforms/qurt/px4_layer/sq_addlast.c | 72 ---- src/platforms/qurt/px4_layer/sq_remfirst.c | 76 ---- src/platforms/qurt/px4_layer/work_cancel.c | 120 ------- src/platforms/qurt/px4_layer/work_lock.h | 53 --- src/platforms/qurt/px4_layer/work_queue.c | 133 ------- src/platforms/qurt/px4_layer/work_thread.c | 328 ------------------ 23 files changed, 2458 deletions(-) delete mode 100644 src/platforms/posix/px4_layer/dq_addlast.c delete mode 100644 src/platforms/posix/px4_layer/dq_rem.c delete mode 100644 src/platforms/posix/px4_layer/dq_remfirst.c delete mode 100644 src/platforms/posix/px4_layer/queue.c delete mode 100644 src/platforms/posix/px4_layer/sq_addafter.c delete mode 100644 src/platforms/posix/px4_layer/sq_addlast.c delete mode 100644 src/platforms/posix/px4_layer/sq_remfirst.c delete mode 100644 src/platforms/posix/px4_layer/work_cancel.c delete mode 100644 src/platforms/posix/px4_layer/work_lock.h delete mode 100644 src/platforms/posix/px4_layer/work_queue.c delete mode 100644 src/platforms/posix/px4_layer/work_thread.c delete mode 100644 src/platforms/qurt/px4_layer/commands_default.c delete mode 100644 src/platforms/qurt/px4_layer/dq_addlast.c delete mode 100644 src/platforms/qurt/px4_layer/dq_rem.c delete mode 100644 src/platforms/qurt/px4_layer/dq_remfirst.c delete mode 100644 src/platforms/qurt/px4_layer/queue.c delete mode 100644 src/platforms/qurt/px4_layer/sq_addafter.c delete mode 100644 src/platforms/qurt/px4_layer/sq_addlast.c delete mode 100644 src/platforms/qurt/px4_layer/sq_remfirst.c delete mode 100644 src/platforms/qurt/px4_layer/work_cancel.c delete mode 100644 src/platforms/qurt/px4_layer/work_lock.h delete mode 100644 src/platforms/qurt/px4_layer/work_queue.c delete mode 100644 src/platforms/qurt/px4_layer/work_thread.c diff --git a/src/platforms/posix/px4_layer/dq_addlast.c b/src/platforms/posix/px4_layer/dq_addlast.c deleted file mode 100644 index 3ef08abd05b6..000000000000 --- a/src/platforms/posix/px4_layer/dq_addlast.c +++ /dev/null @@ -1,74 +0,0 @@ -/************************************************************ - * libc/queue/dq_addlast.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: dq_addlast - * - * Description - * dq_addlast adds 'node' to the end of 'queue' - * - ************************************************************/ - -void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue) -{ - node->flink = NULL; - node->blink = queue->tail; - - if (!queue->head) - { - queue->head = node; - queue->tail = node; - } - else - { - queue->tail->flink = node; - queue->tail = node; - } -} - diff --git a/src/platforms/posix/px4_layer/dq_rem.c b/src/platforms/posix/px4_layer/dq_rem.c deleted file mode 100644 index db20762c758f..000000000000 --- a/src/platforms/posix/px4_layer/dq_rem.c +++ /dev/null @@ -1,84 +0,0 @@ -/************************************************************ - * libc/queue/dq_rem.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: dq_rem - * - * Descripton: - * dq_rem removes 'node' from 'queue' - * - ************************************************************/ - -void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue) -{ - FAR dq_entry_t *prev = node->blink; - FAR dq_entry_t *next = node->flink; - - if (!prev) - { - queue->head = next; - } - else - { - prev->flink = next; - } - - if (!next) - { - queue->tail = prev; - } - else - { - next->blink = prev; - } - - node->flink = NULL; - node->blink = NULL; -} - diff --git a/src/platforms/posix/px4_layer/dq_remfirst.c b/src/platforms/posix/px4_layer/dq_remfirst.c deleted file mode 100644 index e87acc33820b..000000000000 --- a/src/platforms/posix/px4_layer/dq_remfirst.c +++ /dev/null @@ -1,82 +0,0 @@ -/************************************************************ - * libc/queue/dq_remfirst.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: dq_remfirst - * - * Description: - * dq_remfirst removes 'node' from the head of 'queue' - * - ************************************************************/ - -FAR dq_entry_t *dq_remfirst(dq_queue_t *queue) -{ - FAR dq_entry_t *ret = queue->head; - - if (ret) - { - FAR dq_entry_t *next = ret->flink; - if (!next) - { - queue->head = NULL; - queue->tail = NULL; - } - else - { - queue->head = next; - next->blink = NULL; - } - - ret->flink = NULL; - ret->blink = NULL; - } - - return ret; -} - diff --git a/src/platforms/posix/px4_layer/queue.c b/src/platforms/posix/px4_layer/queue.c deleted file mode 100644 index 826cc2d163b8..000000000000 --- a/src/platforms/posix/px4_layer/queue.c +++ /dev/null @@ -1,102 +0,0 @@ -/************************************************************************ - * - * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - * Modified for use in Linux by Mark Charlebois - * - ************************************************************************/ - -// FIXME - need px4_queue -#include -#include - -__EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue); -sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue); -void sq_rem(sq_entry_t *node, sq_queue_t *queue) -{ - if (queue->head && node) - { - if (node == queue->head) - { - queue->head = node->flink; - if (node == queue->tail) - { - queue->tail = NULL; - } - } - else - { - sq_entry_t *prev; - for(prev = (sq_entry_t*)queue->head; - prev && prev->flink != node; - prev = prev->flink); - - if (prev) - { - sq_remafter(prev, queue); - } - } - } -} - -sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue) -{ - sq_entry_t *ret = node->flink; - if (queue->head && ret) - { - if (queue->tail == ret) - { - queue->tail = node; - node->flink = NULL; - } - else - { - node->flink = ret->flink; - } - - ret->flink = NULL; - } - - return ret; -} - -__EXPORT void sq_addfirst(sq_entry_t *node, sq_queue_t *queue); -void sq_addfirst(sq_entry_t *node, sq_queue_t *queue) -{ - node->flink = queue->head; - if (!queue->head) - { - queue->tail = node; - } - queue->head = node; -} - - diff --git a/src/platforms/posix/px4_layer/sq_addafter.c b/src/platforms/posix/px4_layer/sq_addafter.c deleted file mode 100644 index 5d47feba0f71..000000000000 --- a/src/platforms/posix/px4_layer/sq_addafter.c +++ /dev/null @@ -1,71 +0,0 @@ -/************************************************************ - * libc/queue/sq_addafter.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: sq_addafter.c - * - * Description: - * The sq_addafter function adds 'node' after 'prev' in the - * 'queue.' - * - ************************************************************/ - -void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue) -{ - if (!queue->head || prev == queue->tail) - { - sq_addlast(node, queue); - } - else - { - node->flink = prev->flink; - prev->flink = node; - } -} diff --git a/src/platforms/posix/px4_layer/sq_addlast.c b/src/platforms/posix/px4_layer/sq_addlast.c deleted file mode 100644 index faa07efb5ca5..000000000000 --- a/src/platforms/posix/px4_layer/sq_addlast.c +++ /dev/null @@ -1,72 +0,0 @@ -/************************************************************ - * libc/queue/sq_addlast.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: sq_addlast - * - * Description: - * The sq_addlast function places the 'node' at the tail of - * the 'queue' - ************************************************************/ - -void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue) -{ - node->flink = NULL; - if (!queue->head) - { - queue->head = node; - queue->tail = node; - } - else - { - queue->tail->flink = node; - queue->tail = node; - } -} - diff --git a/src/platforms/posix/px4_layer/sq_remfirst.c b/src/platforms/posix/px4_layer/sq_remfirst.c deleted file mode 100644 index f81c18dc2e42..000000000000 --- a/src/platforms/posix/px4_layer/sq_remfirst.c +++ /dev/null @@ -1,76 +0,0 @@ -/************************************************************ - * libc/queue/sq_remfirst.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: sq_remfirst - * - * Description: - * sq_remfirst function removes the first entry from - * 'queue' - * - ************************************************************/ - -FAR sq_entry_t *sq_remfirst(sq_queue_t *queue) -{ - FAR sq_entry_t *ret = queue->head; - - if (ret) - { - queue->head = ret->flink; - if (!queue->head) - { - queue->tail = NULL; - } - - ret->flink = NULL; - } - - return ret; -} - diff --git a/src/platforms/posix/px4_layer/work_cancel.c b/src/platforms/posix/px4_layer/work_cancel.c deleted file mode 100644 index 6f737877d97c..000000000000 --- a/src/platforms/posix/px4_layer/work_cancel.c +++ /dev/null @@ -1,120 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_cancel.c - * - * Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include -#include -#include - -#ifdef CONFIG_SCHED_WORKQUEUE - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: work_cancel - * - * Description: - * Cancel previously queued work. This removes work from the work queue. - * After work has been canceled, it may be re-queue by calling work_queue() - * again. - * - * Input parameters: - * qid - The work queue ID - * work - The previously queue work structure to cancel - * - * Returned Value: - * Zero on success, a negated errno on failure - * - ****************************************************************************/ - -int work_cancel(int qid, struct work_s *work) -{ - struct wqueue_s *wqueue = &g_work[qid]; - //irqstate_t flags; - - //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); - - /* Cancelling the work is simply a matter of removing the work structure - * from the work queue. This must be done with interrupts disabled because - * new work is typically added to the work queue from interrupt handlers. - */ - - //flags = irqsave(); - if (work->worker != NULL) - { - /* A little test of the integrity of the work queue */ - - //DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); - //DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); - - /* Remove the entry from the work queue and make sure that it is - * mark as availalbe (i.e., the worker field is nullified). - */ - - dq_rem((FAR dq_entry_t *)work, &wqueue->q); - work->worker = NULL; - } - - //irqrestore(flags); - return PX4_OK; -} - -#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/px4_layer/work_lock.h b/src/platforms/posix/px4_layer/work_lock.h deleted file mode 100644 index f77f228b36a7..000000000000 --- a/src/platforms/posix/px4_layer/work_lock.h +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -#include -#include - -#pragma once -extern sem_t _work_lock[]; - -inline void work_lock(int id); -inline void work_lock(int id) -{ - //printf("work_lock %d\n", id); - sem_wait(&_work_lock[id]); -} - -inline void work_unlock(int id); -inline void work_unlock(int id) -{ - //printf("work_unlock %d\n", id); - sem_post(&_work_lock[id]); -} - diff --git a/src/platforms/posix/px4_layer/work_queue.c b/src/platforms/posix/px4_layer/work_queue.c deleted file mode 100644 index 46ca35984229..000000000000 --- a/src/platforms/posix/px4_layer/work_queue.c +++ /dev/null @@ -1,133 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_queue.c - * - * Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include "work_lock.h" - -#ifdef CONFIG_SCHED_WORKQUEUE - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: work_queue - * - * Description: - * Queue work to be performed at a later time. All queued work will be - * performed on the worker thread of of execution (not the caller's). - * - * The work structure is allocated by caller, but completely managed by - * the work queue logic. The caller should never modify the contents of - * the work queue structure; the caller should not call work_queue() - * again until either (1) the previous work has been performed and removed - * from the queue, or (2) work_cancel() has been called to cancel the work - * and remove it from the work queue. - * - * Input parameters: - * qid - The work queue ID (index) - * work - The work structure to queue - * worker - The worker callback to be invoked. The callback will invoked - * on the worker thread of execution. - * arg - The argument that will be passed to the workder callback when - * int is invoked. - * delay - Delay (in clock ticks) from the time queue until the worker - * is invoked. Zero means to perform the work immediately. - * - * Returned Value: - * Zero on success, a negated errno on failure - * - ****************************************************************************/ - -int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) -{ - struct wqueue_s *wqueue = &g_work[qid]; - - //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); - - /* First, initialize the work structure */ - - work->worker = worker; /* Work callback */ - work->arg = arg; /* Callback argument */ - work->delay = delay; /* Delay until work performed */ - - /* Now, time-tag that entry and put it in the work queue. This must be - * done with interrupts disabled. This permits this function to be called - * from with task logic or interrupt handlers. - */ - - work_lock(qid); - work->qtime = clock_systimer(); /* Time work queued */ - - dq_addlast((dq_entry_t *)work, &wqueue->q); - px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ - - work_unlock(qid); - return PX4_OK; -} - -#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/px4_layer/work_thread.c b/src/platforms/posix/px4_layer/work_thread.c deleted file mode 100644 index a21584cd0af4..000000000000 --- a/src/platforms/posix/px4_layer/work_thread.c +++ /dev/null @@ -1,328 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_thread.c - * - * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "work_lock.h" - -#ifdef CONFIG_SCHED_WORKQUEUE - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/* The state of each work queue. */ -struct wqueue_s g_work[NWORKERS]; - -/**************************************************************************** - * Private Variables - ****************************************************************************/ -sem_t _work_lock[NWORKERS]; - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: work_process - * - * Description: - * This is the logic that performs actions placed on any work list. - * - * Input parameters: - * wqueue - Describes the work queue to be processed - * - * Returned Value: - * None - * - ****************************************************************************/ - -static void work_process(FAR struct wqueue_s *wqueue, int lock_id) -{ - volatile FAR struct work_s *work; - worker_t worker; - FAR void *arg; - uint64_t elapsed; - uint32_t remaining; - uint32_t next; - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - next = CONFIG_SCHED_WORKPERIOD; - - work_lock(lock_id); - - work = (FAR struct work_s *)wqueue->q.head; - while (work) - { - /* Is this work ready? It is ready if there is no delay or if - * the delay has elapsed. qtime is the time that the work was added - * to the work queue. It will always be greater than or equal to - * zero. Therefore a delay of zero will always execute immediately. - */ - - elapsed = USEC2TICK(clock_systimer() - work->qtime); - //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay); - if (elapsed >= work->delay) - { - /* Remove the ready-to-execute work from the list */ - - (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); - - /* Extract the work description from the entry (in case the work - * instance by the re-used after it has been de-queued). - */ - - worker = work->worker; - arg = work->arg; - - /* Mark the work as no longer being queued */ - - work->worker = NULL; - - /* Do the work. Re-enable interrupts while the work is being - * performed... we don't have any idea how long that will take! - */ - - work_unlock(lock_id); - if (!worker) { - PX4_ERR("MESSED UP: worker = 0"); - } - else { - worker(arg); - } - - /* Now, unfortunately, since we re-enabled interrupts we don't - * know the state of the work list and we will have to start - * back at the head of the list. - */ - - work_lock(lock_id); - work = (FAR struct work_s *)wqueue->q.head; - } - else - { - /* This one is not ready.. will it be ready before the next - * scheduled wakeup interval? - */ - - /* Here: elapsed < work->delay */ - remaining = USEC_PER_TICK*(work->delay - elapsed); - if (remaining < next) - { - /* Yes.. Then schedule to wake up when the work is ready */ - - next = remaining; - } - - /* Then try the next in the list. */ - - work = (FAR struct work_s *)work->dq.flink; - } - } - - /* Wait awhile to check the work list. We will wait here until either - * the time elapses or until we are awakened by a signal. - */ - work_unlock(lock_id); - - usleep(next); -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ -void work_queues_init(void) -{ - sem_init(&_work_lock[HPWORK], 0, 1); - sem_init(&_work_lock[LPWORK], 0, 1); -#ifdef CONFIG_SCHED_USRWORK - sem_init(&_work_lock[USRWORK], 0, 1); -#endif - - // Create high priority worker thread - g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX-1, - 2000, - work_hpthread, - (char* const*)NULL); - - // Create low priority worker thread - g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", - SCHED_DEFAULT, - SCHED_PRIORITY_MIN, - 2000, - work_lpthread, - (char* const*)NULL); - -} - -/**************************************************************************** - * Name: work_hpthread, work_lpthread, and work_usrthread - * - * Description: - * These are the worker threads that performs actions placed on the work - * lists. - * - * work_hpthread and work_lpthread: These are the kernel mode work queues - * (also build in the flat build). One of these threads also performs - * periodic garbage collection (that is otherwise performed by the idle - * thread if CONFIG_SCHED_WORKQUEUE is not defined). - * - * These worker threads are started by the OS during normal bringup. - * - * work_usrthread: This is a user mode work queue. It must be built into - * the applicatino blob during the user phase of a kernel build. The - * user work thread will then automatically be started when the system - * boots by calling through the pointer found in the header on the user - * space blob. - * - * All of these entrypoints are referenced by OS internally and should not - * not be accessed by application logic. - * - * Input parameters: - * argc, argv (not used) - * - * Returned Value: - * Does not return - * - ****************************************************************************/ - -#ifdef CONFIG_SCHED_HPWORK - -int work_hpthread(int argc, char *argv[]) -{ - /* Loop forever */ - - for (;;) - { - /* First, perform garbage collection. This cleans-up memory de-allocations - * that were queued because they could not be freed in that execution - * context (for example, if the memory was freed from an interrupt handler). - * NOTE: If the work thread is disabled, this clean-up is performed by - * the IDLE thread (at a very, very low priority). - */ - -#ifndef CONFIG_SCHED_LPWORK - sched_garbagecollection(); -#endif - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - work_process(&g_work[HPWORK], HPWORK); - } - - return PX4_OK; /* To keep some compilers happy */ -} - -#ifdef CONFIG_SCHED_LPWORK - -int work_lpthread(int argc, char *argv[]) -{ - /* Loop forever */ - - for (;;) - { - /* First, perform garbage collection. This cleans-up memory de-allocations - * that were queued because they could not be freed in that execution - * context (for example, if the memory was freed from an interrupt handler). - * NOTE: If the work thread is disabled, this clean-up is performed by - * the IDLE thread (at a very, very low priority). - */ - - //sched_garbagecollection(); - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - work_process(&g_work[LPWORK], LPWORK); - } - - return PX4_OK; /* To keep some compilers happy */ -} - -#endif /* CONFIG_SCHED_LPWORK */ -#endif /* CONFIG_SCHED_HPWORK */ - -#ifdef CONFIG_SCHED_USRWORK - -int work_usrthread(int argc, char *argv[]) -{ - /* Loop forever */ - - for (;;) - { - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - work_process(&g_work[USRWORK], USRWORK); - } - - return PX4_OK; /* To keep some compilers happy */ -} - -#endif /* CONFIG_SCHED_USRWORK */ - -uint32_t clock_systimer() -{ - //printf("clock_systimer: %0lx\n", hrt_absolute_time()); - return (0x00000000ffffffff & hrt_absolute_time()); -} -#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/qurt/px4_layer/commands_default.c b/src/platforms/qurt/px4_layer/commands_default.c deleted file mode 100644 index 1c4c50f02265..000000000000 --- a/src/platforms/qurt/px4_layer/commands_default.c +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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 commands_default.c - * Commands to run for the "qurt_default" config - * - * @author Mark Charlebois - */ - -const char *get_commands() -{ - static const char *commands = - "hello start\n" - "uorb start\n" - "simulator start -s\n" - "barosim start\n" - "adcsim start\n" - "accelsim start\n" - "gyrosim start\n" - "list_devices\n" - "list_topics\n" - "list_tasks\n" - "param show *\n" - "rgbled start\n" -#if 0 - "sensors start\n" - "param set CAL_GYRO0_ID 2293760\n" - "param set CAL_ACC0_ID 1310720\n" - "hil mode_pwm" - "param set CAL_ACC1_ID 1376256\n" - "param set CAL_MAG0_ID 196608\n" - "mavlink start -d /tmp/ttyS0\n" - "commander start\n" -#endif - ; - - return commands; -} diff --git a/src/platforms/qurt/px4_layer/dq_addlast.c b/src/platforms/qurt/px4_layer/dq_addlast.c deleted file mode 100644 index 3ef08abd05b6..000000000000 --- a/src/platforms/qurt/px4_layer/dq_addlast.c +++ /dev/null @@ -1,74 +0,0 @@ -/************************************************************ - * libc/queue/dq_addlast.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: dq_addlast - * - * Description - * dq_addlast adds 'node' to the end of 'queue' - * - ************************************************************/ - -void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue) -{ - node->flink = NULL; - node->blink = queue->tail; - - if (!queue->head) - { - queue->head = node; - queue->tail = node; - } - else - { - queue->tail->flink = node; - queue->tail = node; - } -} - diff --git a/src/platforms/qurt/px4_layer/dq_rem.c b/src/platforms/qurt/px4_layer/dq_rem.c deleted file mode 100644 index db20762c758f..000000000000 --- a/src/platforms/qurt/px4_layer/dq_rem.c +++ /dev/null @@ -1,84 +0,0 @@ -/************************************************************ - * libc/queue/dq_rem.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: dq_rem - * - * Descripton: - * dq_rem removes 'node' from 'queue' - * - ************************************************************/ - -void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue) -{ - FAR dq_entry_t *prev = node->blink; - FAR dq_entry_t *next = node->flink; - - if (!prev) - { - queue->head = next; - } - else - { - prev->flink = next; - } - - if (!next) - { - queue->tail = prev; - } - else - { - next->blink = prev; - } - - node->flink = NULL; - node->blink = NULL; -} - diff --git a/src/platforms/qurt/px4_layer/dq_remfirst.c b/src/platforms/qurt/px4_layer/dq_remfirst.c deleted file mode 100644 index e87acc33820b..000000000000 --- a/src/platforms/qurt/px4_layer/dq_remfirst.c +++ /dev/null @@ -1,82 +0,0 @@ -/************************************************************ - * libc/queue/dq_remfirst.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: dq_remfirst - * - * Description: - * dq_remfirst removes 'node' from the head of 'queue' - * - ************************************************************/ - -FAR dq_entry_t *dq_remfirst(dq_queue_t *queue) -{ - FAR dq_entry_t *ret = queue->head; - - if (ret) - { - FAR dq_entry_t *next = ret->flink; - if (!next) - { - queue->head = NULL; - queue->tail = NULL; - } - else - { - queue->head = next; - next->blink = NULL; - } - - ret->flink = NULL; - ret->blink = NULL; - } - - return ret; -} - diff --git a/src/platforms/qurt/px4_layer/queue.c b/src/platforms/qurt/px4_layer/queue.c deleted file mode 100644 index eecbd98830bf..000000000000 --- a/src/platforms/qurt/px4_layer/queue.c +++ /dev/null @@ -1,102 +0,0 @@ -/************************************************************************ - * - * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - * Modified for use in Linux by Mark Charlebois - * - ************************************************************************/ - -// FIXME - need px4_queue -#include -#include - -__EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue); -sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue); -void sq_rem(sq_entry_t *node, sq_queue_t *queue) -{ - if (queue->head && node) - { - if (node == queue->head) - { - queue->head = node->flink; - if (node == queue->tail) - { - queue->tail = NULL; - } - } - else - { - sq_entry_t *prev; - for(prev = (sq_entry_t*)queue->head; - prev && prev->flink != node; - prev = prev->flink); - - if (prev) - { - sq_remafter(prev, queue); - } - } - } -} - -sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue) -{ - sq_entry_t *ret = node->flink; - if (queue->head && ret) - { - if (queue->tail == ret) - { - queue->tail = node; - node->flink = NULL; - } - else - { - node->flink = ret->flink; - } - - ret->flink = NULL; - } - - return ret; -} - -__EXPORT void sq_addfirst(sq_entry_t *node, sq_queue_t *queue); -void sq_addfirst(sq_entry_t *node, sq_queue_t *queue) -{ - node->flink = queue->head; - if (!queue->head) - { - queue->tail = node; - } - queue->head = node; -} - - diff --git a/src/platforms/qurt/px4_layer/sq_addafter.c b/src/platforms/qurt/px4_layer/sq_addafter.c deleted file mode 100644 index 5d47feba0f71..000000000000 --- a/src/platforms/qurt/px4_layer/sq_addafter.c +++ /dev/null @@ -1,71 +0,0 @@ -/************************************************************ - * libc/queue/sq_addafter.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: sq_addafter.c - * - * Description: - * The sq_addafter function adds 'node' after 'prev' in the - * 'queue.' - * - ************************************************************/ - -void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue) -{ - if (!queue->head || prev == queue->tail) - { - sq_addlast(node, queue); - } - else - { - node->flink = prev->flink; - prev->flink = node; - } -} diff --git a/src/platforms/qurt/px4_layer/sq_addlast.c b/src/platforms/qurt/px4_layer/sq_addlast.c deleted file mode 100644 index faa07efb5ca5..000000000000 --- a/src/platforms/qurt/px4_layer/sq_addlast.c +++ /dev/null @@ -1,72 +0,0 @@ -/************************************************************ - * libc/queue/sq_addlast.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: sq_addlast - * - * Description: - * The sq_addlast function places the 'node' at the tail of - * the 'queue' - ************************************************************/ - -void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue) -{ - node->flink = NULL; - if (!queue->head) - { - queue->head = node; - queue->tail = node; - } - else - { - queue->tail->flink = node; - queue->tail = node; - } -} - diff --git a/src/platforms/qurt/px4_layer/sq_remfirst.c b/src/platforms/qurt/px4_layer/sq_remfirst.c deleted file mode 100644 index f81c18dc2e42..000000000000 --- a/src/platforms/qurt/px4_layer/sq_remfirst.c +++ /dev/null @@ -1,76 +0,0 @@ -/************************************************************ - * libc/queue/sq_remfirst.c - * - * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ************************************************************/ - -/************************************************************ - * Compilation Switches - ************************************************************/ - -/************************************************************ - * Included Files - ************************************************************/ - -#include - -/************************************************************ - * Public Functions - ************************************************************/ - -/************************************************************ - * Name: sq_remfirst - * - * Description: - * sq_remfirst function removes the first entry from - * 'queue' - * - ************************************************************/ - -FAR sq_entry_t *sq_remfirst(sq_queue_t *queue) -{ - FAR sq_entry_t *ret = queue->head; - - if (ret) - { - queue->head = ret->flink; - if (!queue->head) - { - queue->tail = NULL; - } - - ret->flink = NULL; - } - - return ret; -} - diff --git a/src/platforms/qurt/px4_layer/work_cancel.c b/src/platforms/qurt/px4_layer/work_cancel.c deleted file mode 100644 index 6f737877d97c..000000000000 --- a/src/platforms/qurt/px4_layer/work_cancel.c +++ /dev/null @@ -1,120 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_cancel.c - * - * Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include -#include -#include - -#ifdef CONFIG_SCHED_WORKQUEUE - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: work_cancel - * - * Description: - * Cancel previously queued work. This removes work from the work queue. - * After work has been canceled, it may be re-queue by calling work_queue() - * again. - * - * Input parameters: - * qid - The work queue ID - * work - The previously queue work structure to cancel - * - * Returned Value: - * Zero on success, a negated errno on failure - * - ****************************************************************************/ - -int work_cancel(int qid, struct work_s *work) -{ - struct wqueue_s *wqueue = &g_work[qid]; - //irqstate_t flags; - - //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); - - /* Cancelling the work is simply a matter of removing the work structure - * from the work queue. This must be done with interrupts disabled because - * new work is typically added to the work queue from interrupt handlers. - */ - - //flags = irqsave(); - if (work->worker != NULL) - { - /* A little test of the integrity of the work queue */ - - //DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); - //DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); - - /* Remove the entry from the work queue and make sure that it is - * mark as availalbe (i.e., the worker field is nullified). - */ - - dq_rem((FAR dq_entry_t *)work, &wqueue->q); - work->worker = NULL; - } - - //irqrestore(flags); - return PX4_OK; -} - -#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/qurt/px4_layer/work_lock.h b/src/platforms/qurt/px4_layer/work_lock.h deleted file mode 100644 index f77f228b36a7..000000000000 --- a/src/platforms/qurt/px4_layer/work_lock.h +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -#include -#include - -#pragma once -extern sem_t _work_lock[]; - -inline void work_lock(int id); -inline void work_lock(int id) -{ - //printf("work_lock %d\n", id); - sem_wait(&_work_lock[id]); -} - -inline void work_unlock(int id); -inline void work_unlock(int id) -{ - //printf("work_unlock %d\n", id); - sem_post(&_work_lock[id]); -} - diff --git a/src/platforms/qurt/px4_layer/work_queue.c b/src/platforms/qurt/px4_layer/work_queue.c deleted file mode 100644 index 1746359b634e..000000000000 --- a/src/platforms/qurt/px4_layer/work_queue.c +++ /dev/null @@ -1,133 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_queue.c - * - * Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include "work_lock.h" - -#ifdef CONFIG_SCHED_WORKQUEUE - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: work_queue - * - * Description: - * Queue work to be performed at a later time. All queued work will be - * performed on the worker thread of of execution (not the caller's). - * - * The work structure is allocated by caller, but completely managed by - * the work queue logic. The caller should never modify the contents of - * the work queue structure; the caller should not call work_queue() - * again until either (1) the previous work has been performed and removed - * from the queue, or (2) work_cancel() has been called to cancel the work - * and remove it from the work queue. - * - * Input parameters: - * qid - The work queue ID (index) - * work - The work structure to queue - * worker - The worker callback to be invoked. The callback will invoked - * on the worker thread of execution. - * arg - The argument that will be passed to the workder callback when - * int is invoked. - * delay - Delay (in clock ticks) from the time queue until the worker - * is invoked. Zero means to perform the work immediately. - * - * Returned Value: - * Zero on success, a negated errno on failure - * - ****************************************************************************/ - -int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) -{ - struct wqueue_s *wqueue = &g_work[qid]; - - //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); - - /* First, initialize the work structure */ - - work->worker = worker; /* Work callback */ - work->arg = arg; /* Callback argument */ - work->delay = delay; /* Delay until work performed */ - - /* Now, time-tag that entry and put it in the work queue. This must be - * done with interrupts disabled. This permits this function to be called - * from with task logic or interrupt handlers. - */ - - work_lock(qid); - work->qtime = clock_systimer(); /* Time work queued */ - - dq_addlast((dq_entry_t *)work, &wqueue->q); - px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ - - work_unlock(qid); - return PX4_OK; -} - -#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/qurt/px4_layer/work_thread.c b/src/platforms/qurt/px4_layer/work_thread.c deleted file mode 100644 index 38730341898e..000000000000 --- a/src/platforms/qurt/px4_layer/work_thread.c +++ /dev/null @@ -1,328 +0,0 @@ -/**************************************************************************** - * libc/wqueue/work_thread.c - * - * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "work_lock.h" - -#ifdef CONFIG_SCHED_WORKQUEUE - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/* The state of each work queue. */ -struct wqueue_s g_work[NWORKERS]; - -/**************************************************************************** - * Private Variables - ****************************************************************************/ -sem_t _work_lock[NWORKERS]; - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: work_process - * - * Description: - * This is the logic that performs actions placed on any work list. - * - * Input parameters: - * wqueue - Describes the work queue to be processed - * - * Returned Value: - * None - * - ****************************************************************************/ - -static void work_process(FAR struct wqueue_s *wqueue, int lock_id) -{ - volatile FAR struct work_s *work; - worker_t worker; - FAR void *arg; - uint64_t elapsed; - uint32_t remaining; - uint32_t next; - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - next = CONFIG_SCHED_WORKPERIOD; - - work_lock(lock_id); - - work = (FAR struct work_s *)wqueue->q.head; - while (work) - { - /* Is this work ready? It is ready if there is no delay or if - * the delay has elapsed. qtime is the time that the work was added - * to the work queue. It will always be greater than or equal to - * zero. Therefore a delay of zero will always execute immediately. - */ - - elapsed = USEC2TICK(clock_systimer() - work->qtime); - //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay); - if (elapsed >= work->delay) - { - /* Remove the ready-to-execute work from the list */ - - (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); - - /* Extract the work description from the entry (in case the work - * instance by the re-used after it has been de-queued). - */ - - worker = work->worker; - arg = work->arg; - - /* Mark the work as no longer being queued */ - - work->worker = NULL; - - /* Do the work. Re-enable interrupts while the work is being - * performed... we don't have any idea how long that will take! - */ - - work_unlock(lock_id); - if (!worker) { - PX4_WARN("MESSED UP: worker = 0\n"); - } - else - worker(arg); - - /* Now, unfortunately, since we re-enabled interrupts we don't - * know the state of the work list and we will have to start - * back at the head of the list. - */ - - work_lock(lock_id); - work = (FAR struct work_s *)wqueue->q.head; - } - else - { - /* This one is not ready.. will it be ready before the next - * scheduled wakeup interval? - */ - - /* Here: elapsed < work->delay */ - remaining = USEC_PER_TICK*(work->delay - elapsed); - if (remaining < next) - { - /* Yes.. Then schedule to wake up when the work is ready */ - - next = remaining; - } - - /* Then try the next in the list. */ - - work = (FAR struct work_s *)work->dq.flink; - } - } - - /* Wait awhile to check the work list. We will wait here until either - * the time elapses or until we are awakened by a signal. - */ - work_unlock(lock_id); - - usleep(next); -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ -void work_queues_init(void) -{ - sem_init(&_work_lock[HPWORK], 0, 1); - sem_init(&_work_lock[LPWORK], 0, 1); -#ifdef CONFIG_SCHED_USRWORK - sem_init(&_work_lock[USRWORK], 0, 1); -#endif - - // Create high priority worker thread - g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX-1, - 2000, - work_hpthread, - (char* const*)NULL); - - // Create low priority worker thread - g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", - SCHED_DEFAULT, - SCHED_PRIORITY_MIN, - 2000, - work_lpthread, - (char* const*)NULL); - -} - -/**************************************************************************** - * Name: work_hpthread, work_lpthread, and work_usrthread - * - * Description: - * These are the worker threads that performs actions placed on the work - * lists. - * - * work_hpthread and work_lpthread: These are the kernel mode work queues - * (also build in the flat build). One of these threads also performs - * periodic garbage collection (that is otherwise performed by the idle - * thread if CONFIG_SCHED_WORKQUEUE is not defined). - * - * These worker threads are started by the OS during normal bringup. - * - * work_usrthread: This is a user mode work queue. It must be built into - * the applicatino blob during the user phase of a kernel build. The - * user work thread will then automatically be started when the system - * boots by calling through the pointer found in the header on the user - * space blob. - * - * All of these entrypoints are referenced by OS internally and should not - * not be accessed by application logic. - * - * Input parameters: - * argc, argv (not used) - * - * Returned Value: - * Does not return - * - ****************************************************************************/ - -#ifdef CONFIG_SCHED_HPWORK - -int work_hpthread(int argc, char *argv[]) -{ - /* Loop forever */ - - for (;;) - { - /* First, perform garbage collection. This cleans-up memory de-allocations - * that were queued because they could not be freed in that execution - * context (for example, if the memory was freed from an interrupt handler). - * NOTE: If the work thread is disabled, this clean-up is performed by - * the IDLE thread (at a very, very low priority). - */ - -#ifndef CONFIG_SCHED_LPWORK - sched_garbagecollection(); -#endif - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - work_process(&g_work[HPWORK], HPWORK); - } - - return PX4_OK; /* To keep some compilers happy */ -} - -#ifdef CONFIG_SCHED_LPWORK - -int work_lpthread(int argc, char *argv[]) -{ - /* Loop forever */ - - for (;;) - { - /* First, perform garbage collection. This cleans-up memory de-allocations - * that were queued because they could not be freed in that execution - * context (for example, if the memory was freed from an interrupt handler). - * NOTE: If the work thread is disabled, this clean-up is performed by - * the IDLE thread (at a very, very low priority). - */ - - //sched_garbagecollection(); - - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - work_process(&g_work[LPWORK], LPWORK); - } - - return PX4_OK; /* To keep some compilers happy */ -} - -#endif /* CONFIG_SCHED_LPWORK */ -#endif /* CONFIG_SCHED_HPWORK */ - -#ifdef CONFIG_SCHED_USRWORK - -int work_usrthread(int argc, char *argv[]) -{ - /* Loop forever */ - - for (;;) - { - /* Then process queued work. We need to keep interrupts disabled while - * we process items in the work list. - */ - - work_process(&g_work[USRWORK], USRWORK); - } - - return PX4_OK; /* To keep some compilers happy */ -} - -#endif /* CONFIG_SCHED_USRWORK */ - -uint32_t clock_systimer() -{ - //printf("clock_systimer: %0lx\n", hrt_absolute_time()); - return (0x00000000ffffffff & hrt_absolute_time()); -} -#endif /* CONFIG_SCHED_WORKQUEUE */ From fa82e9d8289191cb4cba53329f3de886e9370e85 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:12:26 +1100 Subject: [PATCH 253/293] uORB: update from upstream --- src/modules/uORB/topics/mission.h | 71 --------------- src/modules/uORB/topics/mission_result.h | 80 ---------------- .../uORB/topics/multirotor_motor_limits.h | 72 --------------- .../uORB/topics/navigation_capabilities.h | 74 --------------- src/modules/uORB/topics/rc_parameter_map.h | 80 ---------------- src/modules/uORB/topics/safety.h | 71 --------------- src/modules/uORB/topics/satellite_info.h | 77 ---------------- src/modules/uORB/topics/telemetry_status.h | 91 ------------------- 8 files changed, 616 deletions(-) delete mode 100644 src/modules/uORB/topics/mission.h delete mode 100644 src/modules/uORB/topics/mission_result.h delete mode 100644 src/modules/uORB/topics/multirotor_motor_limits.h delete mode 100644 src/modules/uORB/topics/navigation_capabilities.h delete mode 100644 src/modules/uORB/topics/rc_parameter_map.h delete mode 100644 src/modules/uORB/topics/safety.h delete mode 100644 src/modules/uORB/topics/satellite_info.h delete mode 100644 src/modules/uORB/topics/telemetry_status.h diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h deleted file mode 100644 index 1a26398e87b3..000000000000 --- a/src/modules/uORB/topics/mission.h +++ /dev/null @@ -1,71 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/mission.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT mission_s { -#else -struct mission_s { -#endif - int32_t dataman_id; - uint32_t count; - int32_t current_seq; -#ifdef __cplusplus - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(mission); diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h deleted file mode 100644 index 9379b57add0d..000000000000 --- a/src/modules/uORB/topics/mission_result.h +++ /dev/null @@ -1,80 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/mission_result.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT mission_result_s { -#else -struct mission_result_s { -#endif - uint32_t instance_count; - uint32_t seq_reached; - uint32_t seq_current; - bool valid; - bool warning; - bool reached; - bool finished; - bool stay_in_failsafe; - bool flight_termination; - bool item_do_jump_changed; - uint32_t item_changed_index; - uint32_t item_do_jump_remaining; -#ifdef __cplusplus - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(mission_result); diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h deleted file mode 100644 index 51a51a3c0dc9..000000000000 --- a/src/modules/uORB/topics/multirotor_motor_limits.h +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/multirotor_motor_limits.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT multirotor_motor_limits_s { -#else -struct multirotor_motor_limits_s { -#endif - uint8_t lower_limit; - uint8_t upper_limit; - uint8_t yaw; - uint8_t reserved; -#ifdef __cplusplus - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(multirotor_motor_limits); diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h deleted file mode 100644 index 67503173ec02..000000000000 --- a/src/modules/uORB/topics/navigation_capabilities.h +++ /dev/null @@ -1,74 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/navigation_capabilities.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT navigation_capabilities_s { -#else -struct navigation_capabilities_s { -#endif - uint64_t timestamp; - float turn_distance; - float landing_horizontal_slope_displacement; - float landing_slope_angle_rad; - float landing_flare_length; - bool abort_landing; -#ifdef __cplusplus - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(navigation_capabilities); diff --git a/src/modules/uORB/topics/rc_parameter_map.h b/src/modules/uORB/topics/rc_parameter_map.h deleted file mode 100644 index cc6e6761f67f..000000000000 --- a/src/modules/uORB/topics/rc_parameter_map.h +++ /dev/null @@ -1,80 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/rc_parameter_map.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus -#define RC_PARAM_MAP_NCHAN 3 -#define PARAM_ID_LEN 16 - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT rc_parameter_map_s { -#else -struct rc_parameter_map_s { -#endif - uint64_t timestamp; - bool valid[3]; - int32_t param_index[3]; - char param_id[51]; - float scale[3]; - float value0[3]; - float value_min[3]; - float value_max[3]; -#ifdef __cplusplus - static const uint8_t RC_PARAM_MAP_NCHAN = 3; - static const uint8_t PARAM_ID_LEN = 16; - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(rc_parameter_map); diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h deleted file mode 100644 index d1ca994ddfed..000000000000 --- a/src/modules/uORB/topics/safety.h +++ /dev/null @@ -1,71 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/safety.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT safety_s { -#else -struct safety_s { -#endif - uint64_t timestamp; - bool safety_switch_available; - bool safety_off; -#ifdef __cplusplus - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(safety); diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h deleted file mode 100644 index f3363b466df4..000000000000 --- a/src/modules/uORB/topics/satellite_info.h +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/satellite_info.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus -#define SAT_INFO_MAX_SATELLITES 20 - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT satellite_info_s { -#else -struct satellite_info_s { -#endif - uint64_t timestamp; - uint8_t count; - uint8_t svid[20]; - uint8_t used[20]; - uint8_t elevation[20]; - uint8_t azimuth[20]; - uint8_t snr[20]; -#ifdef __cplusplus - static const uint8_t SAT_INFO_MAX_SATELLITES = 20; - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(satellite_info); diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h deleted file mode 100644 index 93c27368bd16..000000000000 --- a/src/modules/uORB/topics/telemetry_status.h +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/tridge/project/UAV/APM.xracer/modules/PX4Firmware/msg/telemetry_status.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus -#define TELEMETRY_STATUS_RADIO_TYPE_GENERIC 0 -#define TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO 1 -#define TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET 2 -#define TELEMETRY_STATUS_RADIO_TYPE_WIRE 3 -#define TELEMETRY_STATUS_RADIO_TYPE_USB 4 - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT telemetry_status_s { -#else -struct telemetry_status_s { -#endif - uint64_t timestamp; - uint64_t heartbeat_time; - uint64_t telem_time; - uint8_t type; - uint8_t rssi; - uint8_t remote_rssi; - uint16_t rxerrors; - uint16_t fixed; - uint8_t noise; - uint8_t remote_noise; - uint8_t txbuf; - uint8_t system_id; - uint8_t component_id; -#ifdef __cplusplus - static const uint8_t TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0; - static const uint8_t TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1; - static const uint8_t TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2; - static const uint8_t TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3; - static const uint8_t TELEMETRY_STATUS_RADIO_TYPE_USB = 4; - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(telemetry_status); From fe1fff872892bdd406ad224260a8724186bd7076 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:12:53 +1100 Subject: [PATCH 254/293] update from upstream --- CMakeLists.txt | 672 ++++++++++++++++++++------------------- Firmware.sublime-project | 15 +- LICENSE.md | 4 +- README.md | 15 +- package.xml | 2 + 5 files changed, 371 insertions(+), 337 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bfd7b662b186..6b395b626160 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,332 +1,346 @@ -cmake_minimum_required(VERSION 2.8.3) -project(px4) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -add_definitions(-D__PX4_ROS) -add_definitions(-D__EXPORT=) -add_definitions(-DMAVLINK_DIALECT=common) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - geometry_msgs - message_generation - cmake_modules - gazebo_msgs - sensor_msgs - mav_msgs - libmavconn - tf -) -find_package(Eigen REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - rc_channels.msg - vehicle_attitude.msg - vehicle_attitude_setpoint.msg - manual_control_setpoint.msg - actuator_controls.msg - actuator_controls_0.msg - actuator_controls_virtual_mc.msg - vehicle_rates_setpoint.msg - mc_virtual_rates_setpoint.msg - vehicle_attitude.msg - vehicle_control_mode.msg - actuator_armed.msg - parameter_update.msg - vehicle_status.msg - vehicle_local_position.msg - position_setpoint.msg - position_setpoint_triplet.msg - vehicle_local_position_setpoint.msg - vehicle_global_velocity_setpoint.msg - offboard_control_mode.msg - vehicle_force_setpoint.msg - distance_sensor.msg -) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - gazebo_msgs -) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS src/include - LIBRARIES px4 - CATKIN_DEPENDS message_runtime roscpp rospy std_msgs libmavconn - DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - ${catkin_INCLUDE_DIRS} - src/platforms - src/platforms/ros/px4_messages - src/include - src/modules - src/ - src/lib - ${EIGEN_INCLUDE_DIRS} - integrationtests -) - -## generate multiplatform wrapper headers -## note that the message header files are generated as in any ROS project with generate_messages() -set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_messages) -set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros) -set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary) -set(MULTIPLATFORM_PREFIX px4_) -add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_headers.py - -d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR} - -t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX}) - -## Declare a cpp library -add_library(px4 - src/platforms/ros/px4_ros_impl.cpp - src/platforms/ros/perf_counter.cpp - src/platforms/ros/geo.cpp - src/lib/mathlib/math/Limits.cpp - src/modules/systemlib/circuit_breaker.cpp -) -add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) - -target_link_libraries(px4 - ${catkin_LIBRARIES} -) - -## Declare a test publisher -add_executable(publisher - src/examples/publisher/publisher_main.cpp - src/examples/publisher/publisher_example.cpp) -add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) -target_link_libraries(publisher - ${catkin_LIBRARIES} - px4 -) - -## Declare a test subscriber -add_executable(subscriber - src/examples/subscriber/subscriber_main.cpp - src/examples/subscriber/subscriber_example.cpp) -add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) -target_link_libraries(subscriber - ${catkin_LIBRARIES} - px4 -) - -## MC Attitude Control -add_executable(mc_att_control - src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp - src/modules/mc_att_control_multiplatform/mc_att_control.cpp - src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) -add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(mc_att_control - ${catkin_LIBRARIES} - px4 -) - -## MC Position Control -add_executable(mc_pos_control - src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp - src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp) -add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(mc_pos_control - ${catkin_LIBRARIES} - px4 -) - -## Attitude Estimator dummy -add_executable(attitude_estimator - src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) -add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(attitude_estimator - ${catkin_LIBRARIES} - px4 -) - -## Position Estimator dummy -add_executable(position_estimator - src/platforms/ros/nodes/position_estimator/position_estimator.cpp) -add_dependencies(position_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(position_estimator - ${catkin_LIBRARIES} - px4 -) - -## Manual input -add_executable(manual_input - src/platforms/ros/nodes/manual_input/manual_input.cpp) -add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(manual_input - ${catkin_LIBRARIES} - px4 -) - -## Multicopter Mixer dummy -add_executable(mc_mixer - src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) -add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(mc_mixer - ${catkin_LIBRARIES} - px4 -) - -## Commander dummy -add_executable(commander - src/platforms/ros/nodes/commander/commander.cpp) -add_dependencies(commander ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(commander - ${catkin_LIBRARIES} - px4 -) - -## Mavlink dummy -add_executable(mavlink - src/platforms/ros/nodes/mavlink/mavlink.cpp) -add_dependencies(mavlink ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(mavlink - ${catkin_LIBRARIES} - px4 -) - -## Offboard Position Setpoint Demo -add_executable(demo_offboard_position_setpoints - src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp) -add_dependencies(demo_offboard_position_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(demo_offboard_position_setpoints - ${catkin_LIBRARIES} - px4 -) - -## Offboard Attitude Setpoint Demo -add_executable(demo_offboard_attitude_setpoints - src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp) -add_dependencies(demo_offboard_attitude_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(demo_offboard_attitude_setpoints - ${catkin_LIBRARIES} - px4 -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_px4test.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest(integrationtests/demo_tests/direct_tests.launch) - add_rostest(integrationtests/demo_tests/mavros_tests.launch) +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +#============================================================================= +# CMAKE CODING STANDARD FOR PX4 +# +# Structure +# --------------------------------------------------------------------------- +# +# * Common functions should be included in px_base.cmake. +# +# * OS/ board specific fucntions should be include in +# px_impl_${OS}.cmake or px4_impl_${OS}_${BOARD}.cmake. +# +# Formatting +# --------------------------------------------------------------------------- +# +# * Use hard indents to match the px4 source code. +# +# * All function and script arguments are upper case. +# +# * All local variables are lower case. +# +# * All cmake functions are lowercase. +# +# * For else, endif, endfunction, etc, never put the name of the statement +# +# Instead of the very confusing: +# if (${var} STREQUAL "1") <-- condition now becomes if name +# # do somthing +# elseif (${var} STREQUAL "2") <-- another condition +# # do somthing +# else (${var} STREQUAL "1") <-- tag is referring to name of if +# # do somthing +# endif (${var} STREQUAL "1") <-- tag is referring to name of if +# +# Do this: +# if (${var} STREQUAL "1") <-- condition now becomes if name +# # do somthing +# elseif (${var} STREQUAL "2") <-- another condition +# # do somthing +# else () <-- leave blank +# # do somthing +# endif () <-- leave blank +# +# Functions/Macros +# --------------------------------------------------------------------------- +# +# * Use px4_parse_function_args to parse functions and check for required +# arguments. Unless there is only one argument in the function and it is clear. +# +# * Never use macros. They allow overwriting global variables and this +# makes variable declarations hard to locate. +# +# * If a target from add_custom_* is set in a function, explicitly pass it +# as an output argument so that the target name is clear to the user. +# +# * Avoid use of global variables in functions. Functions in a nested +# scope may use global variables, but this makes it difficult to +# resuse functions. +# +# Included CMake Files +# --------------------------------------------------------------------------- +# +# * All variables in config files must have the prefix "config_". +# +# * Never set global variables in an included cmake file, +# you may only define functions. This excludes config and Toolchain files. +# This makes it clear to the user when variables are being set or targets +# are being created. +# +# * Setting a global variable in a CMakeLists.txt file is ok, because +# each CMakeLists.txt file has scope in the current directory and all +# subdirecties, so it is not truly global. +# +# * All toolchain files should be included in the cmake +# directory and named Toolchain-"name".cmake. +# +# Misc +# --------------------------------------------------------------------------- +# +# * If referencing a string variable, don't put it in quotes. +# Don't do "${OS}" STREQUAL "posix", +# instead type ${OS} STREQUAL "posix". This will throw an +# error when ${OS} is not defined instead of silently +# evaluating to false. +# +#============================================================================= + +# Warning: Changing this modifies CMake's internal workings +# and leads to wrong toolchain detection +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) + +#============================================================================= +# parameters +# + +set(CMAKE_BUILD_TYPE "" CACHE STRING "build type") +set_property(CACHE CMAKE_BUILD_TYPE PROPERTY + STRINGS ";Debug;Release;RelWithDebInfo;MinSizeRel") +set(CONFIG "nuttx_px4fmu-v2_default" CACHE STRING "desired configuration") +file(GLOB_RECURSE configs RELATIVE cmake/configs "cmake/configs/*.cmake") +set_property(CACHE CONFIG PROPERTY STRINGS ${configs}) +set(THREADS "4" CACHE STRING + "number of threads to use for external build processes") +set(DEBUG_PORT "/dev/ttyACM0" CACHE STRING "debugging port") + +#============================================================================= +# configuration +# +# must come before project to set toolchain + +string(REPLACE "_" ";" config_args ${CONFIG}) +list(GET config_args 0 OS) +list(GET config_args 1 BOARD) +list(GET config_args 2 LABEL) +set(target_name "${OS}-${BOARD}-${LABEL}") + +message(STATUS "${target_name}") + +# switch to ros CMake file if building ros +if (${OS} STREQUAL "ros") + include("cmake/ros-CMakeLists.txt") +else() # otherwise use the rest of this file + +list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) +set(config_module "configs/${CONFIG}") +include(${config_module}) + +# cmake modules +include(ExternalProject) + +# require px4 module interface +set(px4_required_interface + px4_os_prebuild_targets + px4_os_add_flags + ) +foreach(cmd ${px4_required_interface}) + if(NOT COMMAND ${cmd}) + message(FATAL_ERROR "${config_module} must implement ${cmd}") + endif() +endforeach() +set(px4_required_config + config_module_list + ) +foreach(conf ${px4_required_config}) + if(NOT DEFINED ${conf}) + message(FATAL_ERROR "cmake/${config_module} must define ${conf}") + endif() +endforeach() + +#============================================================================= +# project definition +# +project(px4 CXX C ASM) +if (NOT ${CMAKE_VERSION} VERSION_LESS 3.0.0) + cmake_policy(SET CMP0045 NEW) # error on non-existent target in get prop + cmake_policy(SET CMP0046 NEW) # no non-existent targets as dependencies + cmake_policy(SET CMP0025 OLD) # still report AppleClang as Clang +endif() +if (NOT ${CMAKE_VERSION} VERSION_LESS 3.1.0) + cmake_policy(SET CMP0054 NEW) # don't dereference quoted variables endif() +set(version_major 1) +set(version_minor 0) +set(version_patch 1) +set(version "${version_major}.${version_minor}.${version_patch}") +set(package-contact "px4users@googlegroups.com") + +#============================================================================= +# programs +# +find_package(PythonInterp REQUIRED) + +#============================================================================= +# cmake modules +# +enable_testing() + +#============================================================================= +# check required toolchain variables +# +set(required_variables + CMAKE_C_COMPILER_ID + ) +foreach(var ${required_variables}) + if (NOT ${var}) + message(FATAL_ERROR "Toolchain/config must define ${var}") + endif() +endforeach() + +#============================================================================= +# git +# +px4_add_git_submodule(TARGET git_genmsg PATH "Tools/genmsg") +px4_add_git_submodule(TARGET git_gencpp PATH "Tools/gencpp") +px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0") +px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest") +px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan") +px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") +px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") +px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") +px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo") +px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix") + +add_custom_target(submodule_clean + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + COMMAND git submodule deinit -f . + COMMAND rm -rf .git/modules/* + ) + +#============================================================================= +# misc targets +# +add_custom_target(check_format + COMMAND Tools/check_code_style.sh + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + +add_custom_target(config + COMMAND cmake-gui . + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} + ) + +#============================================================================= +# external libraries +# +px4_os_prebuild_targets(OUT prebuild_targets + BOARD ${BOARD} + THREADS ${THREADS}) + +#============================================================================= +# build flags +# +px4_os_add_flags( + BOARD ${BOARD} + C_FLAGS c_flags + CXX_FLAGS cxx_flags + EXE_LINKER_FLAGS exe_linker_flags + INCLUDE_DIRS include_dirs + LINK_DIRS link_dirs + DEFINITIONS definitions) + +px4_join(OUT CMAKE_EXE_LINKER_FLAGS LIST "${exe_linker_flags}" GLUE " ") +px4_join(OUT CMAKE_C_FLAGS LIST "${c_flags}" GLUE " ") +px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ") + +include_directories(${include_dirs}) +link_directories(${link_dirs}) +add_definitions(${definitions}) + +#============================================================================= +# source code generation +# +file(GLOB_RECURSE msg_files msg/*.msg) +px4_generate_messages(TARGET msg_gen + MSG_FILES ${msg_files} + OS ${OS} + DEPENDS git_genmsg git_gencpp + ) +px4_generate_parameters_xml(OUT parameters.xml BOARD ${BOARD}) +px4_generate_airframes_xml(OUT airframes.xml BOARD ${BOARD}) +add_custom_target(xml_gen + DEPENDS parameters.xml airframes.xml) + +#============================================================================= +# external projects +# + +set(ep_base ${CMAKE_BINARY_DIR}/external) +set_property(DIRECTORY PROPERTY EP_BASE ${ep_base}) + +# add external project install folders to build +link_directories(${ep_base}/Install/lib) +include_directories(${ep_base}/Install/include) +# add the directories so cmake won't warn +execute_process(COMMAND cmake -E make_directory ${ep_base}/Install/lib) +execute_process(COMMAND cmake -E make_directory ${ep_base}/Install/include) + +#============================================================================= +# subdirectories +# +set(module_libraries) +foreach(module ${config_module_list}) + add_subdirectory(src/${module}) + px4_mangle_name(${module} mangled_name) + list(APPEND module_libraries ${mangled_name}) + #message(STATUS "adding module: ${module}") +endforeach() + +add_subdirectory(src/firmware/${OS}) + +if (config_io_board) + add_subdirectory(src/modules/px4iofirmware) +endif() +#============================================================================= +# generate git version +# +px4_create_git_hash_header(HEADER ${CMAKE_BINARY_DIR}/build_git_version.h) + +#============================================================================= +# packaging +# +# Important to having packaging at end of cmake file. +# +set(CPACK_PACKAGE_VERSION ${version}) +set(CPACK_PACKAGE_CONTACT ${package_contact}) +set(CPACK_GENERATOR "ZIP") +set(CPACK_SOURCE_GENERATOR "ZIP") +include(CPack) + +endif() # ros alternative endif + +# vim: set noet fenc=utf-8 ff=unix ft=cmake : diff --git a/Firmware.sublime-project b/Firmware.sublime-project index b061aa68d8ae..c9cb9cfc0210 100644 --- a/Firmware.sublime-project +++ b/Firmware.sublime-project @@ -33,7 +33,20 @@ { "tab_size": 8, "translate_tabs_to_spaces": false, - "highlight_line": true + "highlight_line": true, + "AStyleFormatter": + { + "options_c": + { + "use_only_additional_options": true, + "additional_options_file": "${project_path}/Tools/astylerc" + }, + "options_c++": + { + "use_only_additional_options": true, + "additional_options_file": "${project_path}/Tools/astylerc" + } + } }, "build_systems": [ diff --git a/LICENSE.md b/LICENSE.md index 2ad83eba48d7..075d48c45291 100644 --- a/LICENSE.md +++ b/LICENSE.md @@ -3,7 +3,7 @@ to be made under the same license. Any exception to this general rule is listed /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,4 +38,4 @@ to be made under the same license. Any exception to this general rule is listed - PX4 middleware: BSD 3-clause - PX4 flight control stack: BSD 3-clause - NuttX operating system: BSD 3-clause - - Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation. \ No newline at end of file + - Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation. diff --git a/README.md b/README.md index 7acae4a6ed36..eb33fff02e40 100644 --- a/README.md +++ b/README.md @@ -4,10 +4,10 @@ [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) -This repository contains the PX4 Flight Core, with the main applications located in the src/modules directory. It also contains the PX4 Drone Platform, which contains drivers and middleware to run drones. +This repository contains the [PX4 Flight Core](http://px4.io), with the main applications located in the src/modules directory. It also contains the PX4 Drone Platform, which contains drivers and middleware to run drones. * Official Website: http://px4.io -* License: BSD 3-clause (see LICENSE.md) +* License: BSD 3-clause (see [LICENSE.md](https://github.com/PX4/Firmware/blob/master/LICENSE.md)) * Supported airframes (more experimental are supported): * [Multicopters](http://px4.io/platforms/multicopters/start) * [Fixed wing](http://px4.io/platforms/planes/start) @@ -16,7 +16,7 @@ This repository contains the PX4 Flight Core, with the main applications located * [Downloads](http://px4.io/firmware/downloads) * Releases * [Downloads](https://github.com/PX4/Firmware/releases) -* Mailing list: [Google Groups](http://groups.google.com/group/px4users) +* Forum / Mailing list: [Google Groups](http://groups.google.com/group/px4users) ### Users ### @@ -28,15 +28,20 @@ Contributing guide: * [CONTRIBUTING.md](https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md) * [PX4 Contribution Guide](http://px4.io/dev/contributing) +Software in the Loop guide: +Use software in the loop [to get started with the codebase](https://pixhawk.org/dev/simulation/native_sitl). + Developer guide: -http://px4.io/dev/ +http://dev.px4.io Testing guide: http://px4.io/dev/unit_tests This repository contains code supporting these boards: + * [Snapdragon Flight](https://www.intrinsyc.com/qualcomm-snapdragon-flight/) * FMUv1.x - * FMUv2.x + * FMUv2.x (Pixhawk) + * FMUv3.x (Pixhawk 2) * AeroCore (v1 and v2) * STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery) diff --git a/package.xml b/package.xml index 2da49096f9de..3d7f17c1f3d3 100644 --- a/package.xml +++ b/package.xml @@ -47,12 +47,14 @@ libmavconn tf rostest + mav_msgs roscpp rospy std_msgs eigen libmavconn tf + mav_msgs From 3fffaa195f270cd261557907ead303f6b74ba8c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:13:15 +1100 Subject: [PATCH 255/293] cmake: update from upstream --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 0b873ccb98f2..850b1a55ea0a 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -115,6 +115,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config From ceb5896cb9860cb6951d5025ed23c3a8e281d2bd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:13:32 +1100 Subject: [PATCH 256/293] nuttx-configs: update from upstream --- .../px4-stm32f4discovery/nsh/defconfig | 2 + nuttx-configs/px4fmu-v1/nsh/defconfig | 20 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 43 +- nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +- nuttx-configs/px4io-v1/scripts/ld.script | 6 + nuttx-configs/px4io-v2/nsh/defconfig | 1128 ++++++++++------- nuttx-configs/px4io-v2/scripts/ld.script | 6 + 7 files changed, 703 insertions(+), 504 deletions(-) diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig index 03092256e9d2..7be5399629e8 100644 --- a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig +++ b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig @@ -263,6 +263,8 @@ CONFIG_STM32_USART=y # # U[S]ART Configuration # +# Hot fix for lost data +CONFIG_STM32_RXDMA_BUFFER_SIZE_OVERRIDE=256 # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART6_RS485 is not set diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index cc34618582ab..62569978464e 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -260,6 +260,8 @@ CONFIG_STM32_USART=y # # U[S]ART Configuration # +# Hot fix for lost data +CONFIG_STM32_RXDMA_BUFFER_SIZE_OVERRIDE=64 # CONFIG_USART1_RS485 is not set # CONFIG_USART1_RXDMA is not set # CONFIG_USART2_RS485 is not set @@ -418,7 +420,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=1000 -CONFIG_USERMAIN_STACKSIZE=3000 +CONFIG_USERMAIN_STACKSIZE=2500 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -504,8 +506,8 @@ CONFIG_MTD_BYTE_WRITE=y # # USART1 Configuration # -CONFIG_USART1_RXBUFSIZE=300 -CONFIG_USART1_TXBUFSIZE=300 +CONFIG_USART1_RXBUFSIZE=128 +CONFIG_USART1_TXBUFSIZE=128 CONFIG_USART1_BAUD=57600 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 @@ -516,8 +518,8 @@ CONFIG_USART1_2STOP=0 # # USART2 Configuration # -CONFIG_USART2_RXBUFSIZE=512 -CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_RXBUFSIZE=300 +CONFIG_USART2_TXBUFSIZE=300 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 @@ -584,8 +586,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=2048 +CONFIG_CDCACM_RXBUFSIZE=300 +CONFIG_CDCACM_TXBUFSIZE=1000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" @@ -720,11 +722,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=1800 +CONFIG_SCHED_WORKSTACKSIZE=1600 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1600 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index c32cc5e3ba96..13ac9f742bac 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -294,6 +294,8 @@ CONFIG_STM32_USART=y # # U[S]ART Configuration # +# Hot fix for lost data +CONFIG_STM32_RXDMA_BUFFER_SIZE_OVERRIDE=256 # CONFIG_USART1_RS485 is not set CONFIG_USART1_RXDMA=y # CONFIG_USART2_RS485 is not set @@ -367,7 +369,8 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=262144 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=1500 +# The actual usage is 420 bytes +CONFIG_ARCH_INTERRUPTSTACK=750 # # Boot options @@ -452,7 +455,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=1000 -CONFIG_USERMAIN_STACKSIZE=3000 +CONFIG_USERMAIN_STACKSIZE=2500 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -548,8 +551,8 @@ CONFIG_UART7_SERIAL_CONSOLE=y # # USART1 Configuration # -CONFIG_USART1_RXBUFSIZE=512 -CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART1_RXBUFSIZE=128 +CONFIG_USART1_TXBUFSIZE=32 CONFIG_USART1_BAUD=115200 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 @@ -561,7 +564,7 @@ CONFIG_USART1_2STOP=0 # USART2 Configuration # CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_TXBUFSIZE=2200 +CONFIG_USART2_TXBUFSIZE=1100 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 @@ -572,8 +575,8 @@ CONFIG_USART2_OFLOWCONTROL=y # # USART3 Configuration # -CONFIG_USART3_RXBUFSIZE=512 -CONFIG_USART3_TXBUFSIZE=512 +CONFIG_USART3_RXBUFSIZE=300 +CONFIG_USART3_TXBUFSIZE=300 CONFIG_USART3_BAUD=57600 CONFIG_USART3_BITS=8 CONFIG_USART3_PARITY=0 @@ -584,8 +587,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=512 -CONFIG_UART4_TXBUFSIZE=512 +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_TXBUFSIZE=300 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -596,8 +599,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=512 -CONFIG_USART6_TXBUFSIZE=512 +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_TXBUFSIZE=300 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -608,8 +611,8 @@ CONFIG_USART6_2STOP=0 # # UART7 Configuration # -CONFIG_UART7_RXBUFSIZE=512 -CONFIG_UART7_TXBUFSIZE=512 +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_TXBUFSIZE=300 CONFIG_UART7_BAUD=57600 CONFIG_UART7_BITS=8 CONFIG_UART7_PARITY=0 @@ -620,8 +623,8 @@ CONFIG_UART7_2STOP=0 # # UART8 Configuration # -CONFIG_UART8_RXBUFSIZE=512 -CONFIG_UART8_TXBUFSIZE=512 +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_TXBUFSIZE=300 CONFIG_UART8_BAUD=57600 CONFIG_UART8_BITS=8 CONFIG_UART8_PARITY=0 @@ -663,8 +666,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=1000 -CONFIG_CDCACM_TXBUFSIZE=8000 +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=4000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" @@ -798,11 +801,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=1800 +CONFIG_SCHED_WORKSTACKSIZE=1600 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1600 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set @@ -1061,3 +1064,5 @@ CONFIG_SYSTEM_SYSINFO=y # # USB Monitor # + +CONFIG_NSOCKET_DESCRIPTORS=0 diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 6c05df3b9886..96cd833d450d 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -246,7 +246,7 @@ CONFIG_STM32_TIM4=y # CONFIG_STM32_TIM5 is not set # CONFIG_STM32_TIM6 is not set # CONFIG_STM32_TIM7 is not set -# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM8=y CONFIG_STM32_TIM9=y CONFIG_STM32_TIM10=y CONFIG_STM32_TIM11=y diff --git a/nuttx-configs/px4io-v1/scripts/ld.script b/nuttx-configs/px4io-v1/scripts/ld.script index 69c2f9cb2ed6..6cc77e9623e1 100755 --- a/nuttx-configs/px4io-v1/scripts/ld.script +++ b/nuttx-configs/px4io-v1/scripts/ld.script @@ -72,6 +72,11 @@ SECTIONS *(.gcc_except_table) *(.gnu.linkonce.r.*) _etext = ABSOLUTE(.); + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; } > flash /* @@ -103,6 +108,7 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); + . = ALIGN(4); } > sram AT > flash .bss : { diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index a9b84779484a..56892e3f9319 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -1,547 +1,725 @@ -############################################################################ -# configs/px4io/nsh/defconfig -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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 NuttX 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. -# -############################################################################ -# -# architecture selection -# -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip family. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization # -CONFIG_ARCH="arm" -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32F100C8=y -CONFIG_ARCH_BOARD="px4io-v2" -CONFIG_ARCH_BOARD_PX4IO_V2=y -CONFIG_BOARD_LOOPSPERMSEC=2000 -CONFIG_DRAM_SIZE=0x00002000 -CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=n -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=y -CONFIG_ARCH_MATH_H=y +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y -CONFIG_ARMV7M_CMNVECTOR=y -CONFIG_ARMV7M_STACKCHECK=n # -# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# Build Setup # -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# CONFIG_EXPERIMENTAL is not set +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + # -# JTAG Enable options: +# Build Configuration # -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + # -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=n -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=y +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y # -# Individual subsystems can be enabled: +# Customize Header Files # -# AHB: -CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=n -CONFIG_STM32_CRC=n -# APB1: -# Timers 2,3 and 4 are owned by the PWM driver -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=n -CONFIG_STM32_TIM4=n -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_WWDG=n -CONFIG_STM32_SPI2=n -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_I2C1=n -CONFIG_STM32_I2C2=n -CONFIG_STM32_BKP=n -CONFIG_STM32_PWR=n -CONFIG_STM32_DAC=n -# APB2: -# We use our own ADC driver, but leave this on for clocking purposes. -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -# TIM1 is owned by the HRT -CONFIG_STM32_TIM1=n -CONFIG_STM32_SPI1=n -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -CONFIG_STM32_ADC3=n +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_DEBUG_SYMBOLS=y +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" # -# STM32F100 specific serial device driver settings +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +# CONFIG_ARMV7M_USEBASEPRI is not set +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +# CONFIG_ARCH_HAVE_FPU is not set +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set + # -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits +# ARMV7M Configuration Options # +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +# CONFIG_ARMV7M_STACKCHECK is not set CONFIG_SERIAL_TERMIOS=y -CONFIG_STANDARD_SERIAL=y -CONFIG_USART1_SERIAL_CONSOLE=y -CONFIG_USART2_SERIAL_CONSOLE=n -CONFIG_USART3_SERIAL_CONSOLE=n +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +CONFIG_ARCH_CHIP_STM32F100C8=y +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F102CB is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +# CONFIG_ARCH_CHIP_STM32F427V is not set +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +CONFIG_STM32_STM32F10XX=y +CONFIG_STM32_VALUELINE=y +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +# CONFIG_STM32_STM32F40XX is not set -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART3_TXBUFSIZE=64 +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_BKP is not set +# CONFIG_STM32_CEC is not set +# CONFIG_STM32_CRC is not set +CONFIG_STM32_DMA1=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_I2C1 is not set +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_PWR is not set +# CONFIG_STM32_SPI1 is not set +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +# CONFIG_STM32_TIM4 is not set +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +# CONFIG_STM32_TIM15 is not set +# CONFIG_STM32_TIM16 is not set +# CONFIG_STM32_TIM17 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_IWDG is not set +# CONFIG_STM32_WWDG is not set +CONFIG_STM32_ADC=y -CONFIG_USART1_RXBUFSIZE=64 -CONFIG_USART2_RXBUFSIZE=64 -CONFIG_USART3_RXBUFSIZE=64 +# +# Alternate Pin Mapping +# +# CONFIG_STM32_USART1_REMAP is not set +# CONFIG_STM32_USART2_REMAP is not set +CONFIG_STM32_USART3_NO_REMAP=y +# CONFIG_STM32_USART3_FULL_REMAP is not set +# CONFIG_STM32_USART3_PARTIAL_REMAP is not set +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +# CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG is not set +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_DMACAPABLE is not set +CONFIG_STM32_USART=y -CONFIG_USART1_BAUD=115200 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 +# +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +# CONFIG_USART2_RS485 is not set +# CONFIG_USART2_RXDMA is not set +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_SERIAL_DISABLE_REORDERING is not set +# CONFIG_STM32_USART_SINGLEWIRE is not set -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 +# +# USB Host Configuration +# -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 +# +# USB Device Configuration +# -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 +# +# External Memory Configuration +# -CONFIG_USART1_RXDMA=y -SERIAL_HAVE_CONSOLE_DMA=y -# Conflicts with I2C1 DMA -CONFIG_USART2_RXDMA=n -CONFIG_USART3_RXDMA=y +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set # -# PX4IO specific driver settings +# Board Settings # -# CONFIG_HRT_TIMER -# Enables the high-resolution timer. The board definition must -# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ -# compare channels to be used. -# CONFIG_HRT_PPM -# Enables R/C PPM input using the HRT. The board definition must -# set HRT_PPM_CHANNEL to the timer capture/compare channel to be -# used, and define GPIO_PPM_IN to configure the appropriate timer -# GPIO. -# CONFIG_PWM_SERVO -# Enables the PWM servo driver. The driver configuration must be -# supplied by the board support at initialisation time. -# Note that USART2 must be disabled on the PX4 board for this to -# be available. +CONFIG_BOARD_LOOPSPERMSEC=2000 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=0x2000 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=500 + # +# Boot options # -CONFIG_HRT_TIMER=y -CONFIG_HRT_PPM=y +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set # -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=n - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 50 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# CONFIG_SCHED_WAITPID - Enable the waitpid() API -# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# Board Selection +# +CONFIG_ARCH_BOARD="px4io-v2" +CONFIG_ARCH_BOARD_PX4IO_V2=y + +# +# Common Board Options # -CONFIG_USER_ENTRYPOINT="user_start" -#CONFIG_APPS_DIR= -CONFIG_DEBUG=n -CONFIG_DEBUG_VERBOSE=n -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEBUG_FS=n -CONFIG_DEBUG_GRAPHICS=n -CONFIG_DEBUG_LCD=n -CONFIG_DEBUG_USB=n -CONFIG_DEBUG_NET=n -CONFIG_DEBUG_RTC=n -CONFIG_DEBUG_ANALOG=n -CONFIG_DEBUG_PWM=n -CONFIG_DEBUG_CAN=n -CONFIG_DEBUG_I2C=n -CONFIG_DEBUG_INPUT=n +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set CONFIG_MSEC_PER_TICK=1 -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_MM_REGIONS=1 -CONFIG_MM_SMALL=y -CONFIG_ARCH_LOWPUTC=y CONFIG_RR_INTERVAL=0 -CONFIG_SCHED_INSTRUMENTATION=n +# CONFIG_SCHED_INSTRUMENTATION is not set CONFIG_TASK_NAME_SIZE=8 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -# this eats ~1KiB of RAM ... work out why CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=n -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=n -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=0 +# CONFIG_MUTEX_TYPES is not set +# CONFIG_PRIORITY_INHERITANCE is not set CONFIG_FDCLONE_DISABLE=y CONFIG_FDCLONE_STDIO=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SCHED_WORKQUEUE=n -CONFIG_SCHED_WORKPRIORITY=50 -CONFIG_SCHED_WORKPERIOD=50000 -CONFIG_SCHED_WORKSTACKSIZE=1024 -CONFIG_SIG_SIGWORK=4 -CONFIG_SCHED_WAITPID=n -CONFIG_SCHED_ATEXIT=n - -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n +# CONFIG_SCHED_WAITPID is not set +# CONFIG_SCHED_STARTHOOK is not set +# CONFIG_SCHED_ATEXIT is not set +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="user_start" +CONFIG_DISABLE_OS_API=y +# CONFIG_DISABLE_CLOCK is not set CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y CONFIG_DISABLE_SIGNALS=y CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y - -# -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# -CONFIG_NOPRINTF_FIELDWIDTH=n - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=n -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n # # Sizes of configurable things (0 disables) # -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled -# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added -# to force automatic, line-oriented flushing the output buffer -# for putc(), fputc(), putchar(), puts(), fputs(), printf(), -# fprintf(), and vfprintf(). When a newline is encountered in -# the output string, the output buffer will be flushed. This -# (slightly) increases the NuttX footprint but supports the kind -# of behavior that people expect for printf(). -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. -# CONFIG_MAX_TASKS=4 CONFIG_MAX_TASK_ARGS=4 CONFIG_NPTHREAD_KEYS=2 CONFIG_NFILE_DESCRIPTORS=8 CONFIG_NFILE_STREAMS=0 CONFIG_NAME_MAX=12 -CONFIG_STDIO_BUFFER_SIZE=32 -CONFIG_STDIO_LINEBUFFER=n -CONFIG_NUNGET_CHARS=2 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 CONFIG_MAX_WDOGPARMS=2 CONFIG_PREALLOC_WDOGS=4 CONFIG_PREALLOC_TIMERS=0 +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=290 +CONFIG_USERMAIN_STACKSIZE=800 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 + +# +# Device Drivers +# +CONFIG_DISABLE_POLL=y +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +# CONFIG_I2C is not set +CONFIG_ARCH_HAVE_I2CRESET=y +# CONFIG_SPI is not set +# CONFIG_RTC is not set +# CONFIG_WATCHDOG is not set +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +# CONFIG_MTD is not set +# CONFIG_PIPES is not set +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_USART1_SERIAL_CONSOLE=y +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART2_BAUD=115200 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USART2_IFLOWCONTROL is not set +# CONFIG_USART2_OFLOWCONTROL is not set + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 +CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +# CONFIG_USART3_IFLOWCONTROL is not set +# CONFIG_USART3_OFLOWCONTROL is not set +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +# CONFIG_USBDEV is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# # -# Settings for apps/nshlib +# System Logging +# +# CONFIG_RAMLOG is not set + # -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). +# Networking Support # +# CONFIG_NET is not set -# Disable NSH completely -CONFIG_NSH_CONSOLE=n +# +# File Systems +# # -# Stack and heap information +# File system configuration # -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3210E-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= -CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=1200 -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_PTHREAD_STACK_DEFAULT=1024 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= +CONFIG_DISABLE_MOUNTPOINT=y +# CONFIG_FS_RAMMAP is not set + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +CONFIG_MM_SMALL=y +CONFIG_MM_REGIONS=1 +# CONFIG_GRAN is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +# CONFIG_STDIO_LINEBUFFER is not set +CONFIG_NUNGET_CHARS=2 +# CONFIG_NOPRINTF_FIELDWIDTH is not set +# CONFIG_LIBC_FLOATINGPOINT is not set +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +# CONFIG_LIBC_STRERROR is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set + +# +# Non-standard Library Support +# +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +# CONFIG_C99_BOOL8 is not set +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +# CONFIG_EXAMPLES_MOUNT is not set +# CONFIG_EXAMPLES_NRF24L01TERM is not set +# CONFIG_EXAMPLES_NSH is not set +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POLL is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +# CONFIG_NSH_LIBRARY is not set + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# readline() +# +# CONFIG_SYSTEM_READLINE is not set + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +# CONFIG_SYSTEM_SYSINFO is not set + +# +# USB Monitor +# + +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y + diff --git a/nuttx-configs/px4io-v2/scripts/ld.script b/nuttx-configs/px4io-v2/scripts/ld.script index 69c2f9cb2ed6..6cc77e9623e1 100755 --- a/nuttx-configs/px4io-v2/scripts/ld.script +++ b/nuttx-configs/px4io-v2/scripts/ld.script @@ -72,6 +72,11 @@ SECTIONS *(.gcc_except_table) *(.gnu.linkonce.r.*) _etext = ABSOLUTE(.); + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; } > flash /* @@ -103,6 +108,7 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); + . = ALIGN(4); } > sram AT > flash .bss : { From 24e0c1b914aacd1e93b6a63fdf8945323ca24ae4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:13:42 +1100 Subject: [PATCH 257/293] nuttx-configs: update from upstream --- nuttx-configs/aerocore/nsh/defconfig | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index e314bf1d8207..655dd1406e83 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -268,6 +268,8 @@ CONFIG_STM32_USART=y # # U[S]ART Configuration # +# Hot fix for lost data +CONFIG_STM32_RXDMA_BUFFER_SIZE_OVERRIDE=256 # CONFIG_USART1_RS485 is not set CONFIG_USART1_RXDMA=y # CONFIG_USART2_RS485 is not set From 88ad1d7f34d7bebc61a1cdc2c5c05ff3f89db07a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 13:13:52 +1100 Subject: [PATCH 258/293] include: update from upstream --- src/include/containers/List.hpp | 20 ++++++++++++-------- src/include/mavlink/mavlink_log.h | 24 ++++++++++++------------ 2 files changed, 24 insertions(+), 20 deletions(-) diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 1906030abb5b..6046502e3f2d 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -43,31 +43,35 @@ template class __EXPORT ListNode { public: - ListNode() : _sibling(nullptr) { + ListNode() : _sibling(nullptr) + { } virtual ~ListNode() {}; void setSibling(T sibling) { _sibling = sibling; } T getSibling() { return _sibling; } - T get() { + T get() + { return _sibling; } protected: T _sibling; private: // forbid copy - ListNode(const ListNode& other); + ListNode(const ListNode &other); // forbid assignment - ListNode & operator = (const ListNode &); + ListNode &operator = (const ListNode &); }; template class __EXPORT List { public: - List() : _head() { + List() : _head() + { } virtual ~List() {}; - void add(T newNode) { + void add(T newNode) + { newNode->setSibling(getHead()); setHead(newNode); } @@ -77,7 +81,7 @@ class __EXPORT List T _head; private: // forbid copy - List(const List& other); + List(const List &other); // forbid assignment - List& operator = (const List &); + List &operator = (const List &); }; diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 4ba2f98ad530..f0141d8a1793 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -106,10 +106,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_emergency(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); /** * Send a mavlink critical message and print to console. @@ -117,10 +117,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_critical(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); /** * Send a mavlink emergency message and print to console. @@ -128,10 +128,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_info(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); struct mavlink_logmessage { char text[MAVLINK_LOG_MAXLEN + 1]; From 92d8242943437866ec1ec27b3d13b5237d98e490 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 08:21:26 +1100 Subject: [PATCH 259/293] build: cleanup topics dir between builds --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 36e473c0e28f..081e912e62f8 100644 --- a/Makefile +++ b/Makefile @@ -264,10 +264,10 @@ generateuorbtopicheaders: checksubmodules $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR)) @$(ECHO) "Generating multiplatform uORB topic wrapper headers" + $(Q) (rm -r $(TOPICHEADER_TEMP_DIR)) $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ -d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX)) -# clean up temporary files $(Q) (rm -r $(TOPICHEADER_TEMP_DIR)) # From 5d2ec71a8e9e4b8e39f9ca830cbb9aafdad93d98 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 08:21:47 +1100 Subject: [PATCH 260/293] merge: removed some files that are gone in upstream --- Tools/posix_run.sh | 19 - posix-configs/posixtest/init/rc.S | 16 - posix-configs/posixtest/scripts/ld.script | 46 -- .../flow_position_estimator_main.c | 465 ------------------ .../flow_position_estimator_params.c | 72 --- .../flow_position_estimator_params.h | 68 --- .../flow_position_estimator/module.mk | 43 -- 7 files changed, 729 deletions(-) delete mode 100755 Tools/posix_run.sh delete mode 100644 posix-configs/posixtest/init/rc.S delete mode 100644 posix-configs/posixtest/scripts/ld.script delete mode 100644 src/examples/flow_position_estimator/flow_position_estimator_main.c delete mode 100644 src/examples/flow_position_estimator/flow_position_estimator_params.c delete mode 100644 src/examples/flow_position_estimator/flow_position_estimator_params.h delete mode 100644 src/examples/flow_position_estimator/module.mk diff --git a/Tools/posix_run.sh b/Tools/posix_run.sh deleted file mode 100755 index fdeefb4a29ae..000000000000 --- a/Tools/posix_run.sh +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/bash - -if [ ! -d /fs/msdcard ] && [ ! -w /fs/msdcard ] - then - echo "Need to create/mount writable /fs/microsd" - echo "sudo mkdir -p /fs/msdcard" - echo "sudo chmod 777 /fs/msdcard" - exit 1 -fi - -if [ ! -d /eeprom ] && [ ! -w /eeprom ] - then - echo "Need to create/mount writable /eeprom" - echo "sudo mkdir -p /eeprom" - echo "sudo chmod 777 /eeprom" - exit 1 -fi - -Build/posix_default.build/mainapp posix-configs/posixtest/init/rc.S diff --git a/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S deleted file mode 100644 index 896062639ea3..000000000000 --- a/posix-configs/posixtest/init/rc.S +++ /dev/null @@ -1,16 +0,0 @@ -uorb start -simulator start -s -barosim start -adcsim start -accelsim start -gyrosim start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1310720 -param set CAL_ACC1_ID 1376256 -param set CAL_MAG0_ID 196608 -rgbled start -mavlink start -d /tmp/ttyS0 -sensors start -hil mode_pwm -commander start -list_devices diff --git a/posix-configs/posixtest/scripts/ld.script b/posix-configs/posixtest/scripts/ld.script deleted file mode 100644 index 32478e1e1411..000000000000 --- a/posix-configs/posixtest/scripts/ld.script +++ /dev/null @@ -1,46 +0,0 @@ -/**************************************************************************** - * ld.script - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * Author: Mark Charlebois - * - * 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. - * - ****************************************************************************/ - -SECTIONS -{ - /* - * Construction data for parameters. - */ - __param : ALIGN(8) { - __param_start = .; - KEEP(*(__param*)) - __param_end = .; - } -} diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c deleted file mode 100644 index 027511df2069..000000000000 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ /dev/null @@ -1,465 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file flow_position_estimator_main.c - * - * Optical flow position estimator - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "flow_position_estimator_params.h" - -__EXPORT int flow_position_estimator_main(int argc, char *argv[]); -static bool thread_should_exit = false; /**< Daemon exit flag */ -static bool thread_running = false; /**< Daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ - -int flow_position_estimator_thread_main(int argc, char *argv[]); -static void usage(const char *reason); - -static void usage(const char *reason) -{ - if (reason) { - fprintf(stderr, "%s\n", reason); - } - - fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The daemon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to px4_task_spawn_cmd(). - */ -int flow_position_estimator_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage("missing command"); - } - - if (!strcmp(argv[1], "start")) { - if (thread_running) { - printf("flow position estimator already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = px4_task_spawn_cmd("flow_position_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 4000, - flow_position_estimator_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tflow position estimator is running\n"); - - } else { - printf("\tflow position estimator not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int flow_position_estimator_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - thread_running = true; - printf("[flow position estimator] starting\n"); - - /* rotation matrix for transformation of optical flow speed vectors */ - static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 }, - { 1, 0, 0 }, - { 0, 0, 1 } - }; // 90deg rotated - const float time_scale = powf(10.0f, -6.0f); - static float speed[3] = {0.0f, 0.0f, 0.0f}; - static float flow_speed[3] = {0.0f, 0.0f, 0.0f}; - static float global_speed[3] = {0.0f, 0.0f, 0.0f}; - static uint32_t counter = 0; - static uint64_t time_last_flow = 0; // in ms - static float dt = 0.0f; // seconds - static float sonar_last = 0.0f; - static bool sonar_valid = false; - static float sonar_lp = 0.0f; - - /* subscribe to vehicle status, attitude, sensors and flow*/ - struct actuator_armed_s armed; - memset(&armed, 0, sizeof(armed)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct optical_flow_s flow; - memset(&flow, 0, sizeof(flow)); - - /* subscribe to parameter changes */ - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - - /* subscribe to armed topic */ - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - - /* subscribe to safety topic */ - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - - /* subscribe to attitude */ - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - - /* subscribe to attitude setpoint */ - int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - - /* subscribe to optical flow*/ - int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); - - /* init local position and filtered flow struct */ - struct vehicle_local_position_s local_pos = { - .x = 0.0f, - .y = 0.0f, - .z = 0.0f, - .vx = 0.0f, - .vy = 0.0f, - .vz = 0.0f - }; - struct filtered_bottom_flow_s filtered_flow = { - .sumx = 0.0f, - .sumy = 0.0f, - .vx = 0.0f, - .vy = 0.0f - }; - - /* advert pub messages */ - orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); - orb_advert_t filtered_flow_pub = orb_advertise(ORB_ID(filtered_bottom_flow), &filtered_flow); - - /* vehicle flying status parameters */ - bool vehicle_liftoff = false; - bool sensors_ready = false; - - /* parameters init*/ - struct flow_position_estimator_params params; - struct flow_position_estimator_param_handles param_handles; - parameters_init(¶m_handles); - parameters_update(¶m_handles, ¶ms); - - perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_estimator_runtime"); - perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval"); - perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err"); - - while (!thread_should_exit) { - - if (sensors_ready) { - /*This runs at the rate of the sensors */ - struct pollfd fds[2] = { - { .fd = optical_flow_sub, .events = POLLIN }, - { .fd = parameter_update_sub, .events = POLLIN } - }; - - /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 2, 500); - - if (ret < 0) { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - - } else if (ret == 0) { - /* no return value, ignore */ -// printf("[flow position estimator] no bottom flow.\n"); - } else { - - /* parameter update available? */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - - parameters_update(¶m_handles, ¶ms); - printf("[flow position estimator] parameters updated.\n"); - } - - /* only if flow data changed */ - if (fds[0].revents & POLLIN) { - perf_begin(mc_loop_perf); - - orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - /* got flow, updating attitude and status as well */ - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - - /* vehicle state estimation */ - float sonar_new = flow.ground_distance_m; - - /* set liftoff boolean - * -> at bottom sonar sometimes does not work correctly, and has to be calibrated (distance higher than 0.3m) - * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time) - * -> minimum sonar value 0.3m - */ - - if (!vehicle_liftoff) { - if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) { - vehicle_liftoff = true; - } - - } else { - if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) { - vehicle_liftoff = false; - } - } - - /* calc dt between flow timestamps */ - /* ignore first flow msg */ - if (time_last_flow == 0) { - time_last_flow = flow.timestamp; - continue; - } - - dt = (float)(flow.timestamp - time_last_flow) * time_scale ; - time_last_flow = flow.timestamp; - - /* only make position update if vehicle is lift off or DEBUG is activated*/ - if (vehicle_liftoff || params.debug) { - /* copy flow */ - if (flow.integration_timespan > 0) { - 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; - - } else { - flow_speed[0] = 0; - flow_speed[1] = 0; - } - - flow_speed[2] = 0.0f; - - /* convert to bodyframe velocity */ - for (uint8_t i = 0; i < 3; i++) { - float sum = 0.0f; - - for (uint8_t j = 0; j < 3; j++) { - sum = sum + flow_speed[j] * rotM_flow_sensor[j][i]; - } - - speed[i] = sum; - } - - /* update filtered flow */ - filtered_flow.sumx = filtered_flow.sumx + speed[0] * dt; - filtered_flow.sumy = filtered_flow.sumy + speed[1] * dt; - filtered_flow.vx = speed[0]; - filtered_flow.vy = speed[1]; - - // TODO add yaw rotation correction (with distance to vehicle zero) - - /* convert to globalframe velocity - * -> local position is currently not used for position control - */ - for (uint8_t i = 0; i < 3; i++) { - float sum = 0.0f; - - for (uint8_t j = 0; j < 3; j++) { - sum = sum + speed[j] * PX4_R(att.R, i, j); - } - - global_speed[i] = sum; - } - - local_pos.x = local_pos.x + global_speed[0] * dt; - local_pos.y = local_pos.y + global_speed[1] * dt; - local_pos.vx = global_speed[0]; - local_pos.vy = global_speed[1]; - local_pos.xy_valid = true; - local_pos.v_xy_valid = true; - - } else { - /* set speed to zero and let position as it is */ - filtered_flow.vx = 0; - filtered_flow.vy = 0; - local_pos.vx = 0; - local_pos.vy = 0; - local_pos.xy_valid = false; - local_pos.v_xy_valid = false; - } - - /* filtering ground distance */ - if (!vehicle_liftoff || !armed.armed) { - /* not possible to fly */ - sonar_valid = false; - local_pos.z = 0.0f; - local_pos.z_valid = false; - - } else { - sonar_valid = true; - } - - if (sonar_valid || params.debug) { - /* simple lowpass sonar filtering */ - /* if new value or with sonar update frequency */ - if (sonar_new != sonar_last || counter % 10 == 0) { - sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp; - sonar_last = sonar_new; - } - - float height_diff = sonar_new - sonar_lp; - - /* if over 1/2m spike follow lowpass */ - if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) { - local_pos.z = -sonar_lp; - - } else { - local_pos.z = -sonar_new; - } - - local_pos.z_valid = true; - } - - filtered_flow.timestamp = hrt_absolute_time(); - local_pos.timestamp = hrt_absolute_time(); - - /* publish local position */ - if (isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) - && isfinite(local_pos.vx) && isfinite(local_pos.vy)) { - orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); - } - - /* publish filtered flow */ - if (isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) - && isfinite(filtered_flow.vy)) { - orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow); - } - - /* measure in what intervals the position estimator runs */ - perf_count(mc_interval_perf); - perf_end(mc_loop_perf); - - } - } - - } else { - /* sensors not ready waiting for first attitude msg */ - - /* polling */ - struct pollfd fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; - - /* wait for a attitude message, check for exit condition every 5 s */ - int ret = poll(fds, 1, 5000); - - if (ret < 0) { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - - } else if (ret == 0) { - /* no return value, ignore */ - printf("[flow position estimator] no attitude received.\n"); - - } else { - if (fds[0].revents & POLLIN) { - sensors_ready = true; - printf("[flow position estimator] initialized.\n"); - } - } - } - - counter++; - } - - printf("[flow position estimator] exiting.\n"); - thread_running = false; - - close(vehicle_attitude_setpoint_sub); - close(vehicle_attitude_sub); - close(armed_sub); - close(control_mode_sub); - close(parameter_update_sub); - close(optical_flow_sub); - - perf_print_counter(mc_loop_perf); - perf_free(mc_loop_perf); - - fflush(stdout); - return 0; -} - - diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.c b/src/examples/flow_position_estimator/flow_position_estimator_params.c deleted file mode 100644 index b56579787473..000000000000 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.c +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file flow_position_estimator_params.c - * - * Parameters for position estimator - */ - -#include "flow_position_estimator_params.h" - -/* Extended Kalman Filter covariances */ - -/* controller parameters */ -PARAM_DEFINE_FLOAT(FPE_LO_THRUST, 0.4f); -PARAM_DEFINE_FLOAT(FPE_SONAR_LP_U, 0.5f); -PARAM_DEFINE_FLOAT(FPE_SONAR_LP_L, 0.2f); -PARAM_DEFINE_INT32(FPE_DEBUG, 0); - - -int parameters_init(struct flow_position_estimator_param_handles *h) -{ - /* PID parameters */ - h->minimum_liftoff_thrust = param_find("FPE_LO_THRUST"); - h->sonar_upper_lp_threshold = param_find("FPE_SONAR_LP_U"); - h->sonar_lower_lp_threshold = param_find("FPE_SONAR_LP_L"); - h->debug = param_find("FPE_DEBUG"); - - return OK; -} - -int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p) -{ - param_get(h->minimum_liftoff_thrust, &(p->minimum_liftoff_thrust)); - param_get(h->sonar_upper_lp_threshold, &(p->sonar_upper_lp_threshold)); - param_get(h->sonar_lower_lp_threshold, &(p->sonar_lower_lp_threshold)); - param_get(h->debug, &(p->debug)); - - return OK; -} diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.h b/src/examples/flow_position_estimator/flow_position_estimator_params.h deleted file mode 100644 index 3ab4e560fc97..000000000000 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.h +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file flow_position_estimator_params.h - * - * Parameters for position estimator - */ - -#include - -struct flow_position_estimator_params { - float minimum_liftoff_thrust; - float sonar_upper_lp_threshold; - float sonar_lower_lp_threshold; - int debug; -}; - -struct flow_position_estimator_param_handles { - param_t minimum_liftoff_thrust; - param_t sonar_upper_lp_threshold; - param_t sonar_lower_lp_threshold; - param_t debug; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct flow_position_estimator_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p); diff --git a/src/examples/flow_position_estimator/module.mk b/src/examples/flow_position_estimator/module.mk deleted file mode 100644 index 5c6e29f8f61c..000000000000 --- a/src/examples/flow_position_estimator/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Build position estimator -# - -MODULE_COMMAND = flow_position_estimator - -SRCS = flow_position_estimator_main.c \ - flow_position_estimator_params.c - -EXTRACFLAGS = -Wno-float-equal From 57f25a5ef7f374f28cc66c86e5e3ae038733b709 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 08:29:20 +1100 Subject: [PATCH 261/293] systemlib: added printload.c to the build --- 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 a205753c4322..5099562d1504 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -50,6 +50,7 @@ SRCS = \ mcu_version.c \ bson/tinybson.c \ circuit_breaker.cpp \ + printload.c \ $(BUILD_DIR)git_version.c ifneq ($(ARDUPILOT_BUILD),1) From cbd2802a486e5d8b32cd2aced1208e09de521e23 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 08:29:37 +1100 Subject: [PATCH 262/293] motor_test: fixed build this is C, not C++ --- src/systemcmds/motor_test/motor_test.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 3581c2004bd9..a1175c289f64 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -59,7 +59,7 @@ __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 = nullptr; +static orb_advert_t _test_motor_pub = NULL; void motor_test(unsigned channel, float value) { @@ -69,7 +69,7 @@ void motor_test(unsigned channel, float value) _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub != nullptr) { + if (_test_motor_pub != NULL) { /* publish test state */ orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); From 73988109917a2bb6ca305c72d7555e59ff101d28 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 08:29:55 +1100 Subject: [PATCH 263/293] ver: fixed build with makefile based build system --- src/systemcmds/ver/ver.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 28cd9886278e..6040e0cd92a4 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -58,9 +58,6 @@ static const char sz_ver_all_str[] = "all"; static const char mcu_ver_str[] = "mcu"; static const char mcu_uid_str[] = "uid"; -const char *px4_git_version = PX4_GIT_VERSION_STR; -const uint64_t px4_git_version_binary = PX4_GIT_VERSION_BINARY; - static void usage(const char *reason) { if (reason != NULL) { From 876374bbe4d97320a176b5170fbc46863c15a8ab Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 08:46:14 +1100 Subject: [PATCH 264/293] rename Makefile to Makefile.make preparing for cmake based build --- Makefile.make | 371 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 371 insertions(+) create mode 100644 Makefile.make diff --git a/Makefile.make b/Makefile.make new file mode 100644 index 000000000000..081e912e62f8 --- /dev/null +++ b/Makefile.make @@ -0,0 +1,371 @@ +# +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# 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. +# + +# +# Top-level Makefile for building PX4 firmware images. +# + +TARGETS := nuttx posix qurt +EXPLICIT_TARGET := $(filter $(TARGETS),$(MAKECMDGOALS)) +ifneq ($(EXPLICIT_TARGET),) + export PX4_TARGET_OS=$(EXPLICIT_TARGET) + export GOALS := $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) +endif + +# +# Get path and tool configuration +# +export PX4_BASE := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/ +include $(PX4_BASE)makefiles/setup.mk + +# +# Get a version string provided by git +# This assumes that git command is available and that +# the directory holding this file also contains .git directory +# +GIT_DESC := $(shell git log -1 --pretty=format:%H) +ifneq ($(words $(GIT_DESC)),1) + GIT_DESC := "unknown_git_version" +endif + +GIT_DESC_SHORT := $(shell echo $(GIT_DESC) | cut -c1-16) + +$(shell mkdir -p $(BUILD_DIR)) +$(shell rm -f $(BUILD_DIR)git_version.*) +$(shell echo "#include " > $(BUILD_DIR)git_version.c) +$(shell echo "const char* px4_git_version = \"$(GIT_DESC)\";" >> $(BUILD_DIR)git_version.c) +$(shell echo "const uint64_t px4_git_version_binary = 0x$(GIT_DESC_SHORT);" >> $(BUILD_DIR)git_version.c) + +# +# Canned firmware configurations that we (know how to) build. +# +KNOWN_CONFIGS := $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)/$(PX4_TARGET_OS)/config_*.mk)))) +CONFIGS ?= $(KNOWN_CONFIGS) + +# +# Boards that we (know how to) build NuttX export kits for. +# +KNOWN_BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)/$(PX4_TARGET_OS)/board_*.mk)))) +BOARDS ?= $(KNOWN_BOARDS) + +# +# Debugging +# +MQUIET = --no-print-directory +#MQUIET = --print-directory + +################################################################################ +# No user-serviceable parts below +################################################################################ + +# +# If the user has listed a config as a target, strip it out and override CONFIGS. +# +FIRMWARE_GOAL = firmware +EXPLICIT_CONFIGS := $(filter $(CONFIGS),$(MAKECMDGOALS)) +ifneq ($(EXPLICIT_CONFIGS),) +CONFIGS := $(EXPLICIT_CONFIGS) +.PHONY: $(EXPLICIT_CONFIGS) +$(EXPLICIT_CONFIGS): all + +# +# If the user has asked to upload, they must have also specified exactly one +# config. +# +ifneq ($(filter upload,$(MAKECMDGOALS)),) +ifneq ($(words $(EXPLICIT_CONFIGS)),1) +$(error In order to upload, exactly one board config must be specified) +endif +FIRMWARE_GOAL = upload +.PHONY: upload +upload: + @: +endif +endif + +ifeq ($(PX4_TARGET_OS),nuttx) +# +# Built products +# +DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) +STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) +FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) + +all: $(DESIRED_FIRMWARES) + +# +# Copy FIRMWARES into the image directory. +# +# XXX copying the .bin files is a hack to work around the PX4IO uploader +# not supporting .px4 files, and it should be deprecated onced that +# is taken care of. +# +$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 + @$(ECHO) %% Copying $@ + $(Q) $(COPY) $< $@ + $(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@) + +# +# Generate FIRMWARES. +# +.PHONY: $(FIRMWARES) +$(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) +$(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ +$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: generateuorbtopicheaders checksubmodules + @$(ECHO) %%%% + @$(ECHO) %%%% Building $(config) in $(work_dir) + @$(ECHO) %%%% + $(Q) $(MKDIR) -p $(work_dir) + $(Q)+ $(MAKE) -r -C $(work_dir) \ + -f $(PX4_MK_DIR)firmware.mk \ + CONFIG=$(config) \ + WORK_DIR=$(work_dir) \ + $(FIRMWARE_GOAL) + +# +# Make FMU firmwares depend on the corresponding IO firmware. +# +# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency +# and forces the _default config in all cases. There has to be a better way to do this... +# +FMU_VERSION = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1)))) +define FMU_DEP +$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4 +endef +FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS)) +# $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) + +# +# Build the NuttX export archives. +# +# Note that there are no explicit dependencies extended from these +# archives. If NuttX is updated, the user is expected to rebuild the +# archives/build area manually. Likewise, when the 'archives' target is +# invoked, all archives are always rebuilt. +# +# XXX Should support fetching/unpacking from a separate directory to permit +# downloads of the prebuilt archives as well... +# +NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) +.PHONY: archives +archives: checksubmodules $(NUTTX_ARCHIVES) + +$(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) +$(ARCHIVE_DIR)%.export: configuration = nsh +$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) + @$(ECHO) %% Configuring NuttX for $(board) + $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) + $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) + @$(ECHO) %% Exporting NuttX for $(board) + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q) $(MKDIR) -p $(dir $@) + $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ + $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) + +# +# The user can run the NuttX 'menuconfig' tool for a single board configuration with +# make BOARDS= menuconfig +# +ifeq ($(MAKECMDGOALS),menuconfig) +ifneq ($(words $(BOARDS)),1) +$(error BOARDS must specify exactly one board for the menuconfig goal) +endif +BOARD = $(BOARDS) +menuconfig: $(NUTTX_SRC) + @$(ECHO) %% Configuring NuttX for $(BOARD) + $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) + $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) + @$(ECHO) %% Running menuconfig for $(BOARD) + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + @$(ECHO) %% Saving configuration file + $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig +else +menuconfig: + @$(ECHO) "" + @$(ECHO) "The menuconfig goal must be invoked without any other goal being specified" + @$(ECHO) "" + +endif + +$(NUTTX_SRC): checksubmodules + +$(UAVCAN_DIR): + $(Q) (./Tools/check_submodules.sh) + +endif + +ifeq ($(PX4_TARGET_OS),nuttx) +# TODO +# Move the above nuttx specific rules into $(PX4_BASE)makefiles/firmware_nuttx.mk +endif +ifeq ($(PX4_TARGET_OS),posix) +include $(PX4_BASE)makefiles/firmware_posix.mk +endif +ifeq ($(PX4_TARGET_OS),qurt) +include $(PX4_BASE)makefiles/firmware_qurt.mk +endif + + +.PHONY: checksubmodules +checksubmodules: + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) + +.PHONY: updatesubmodules +updatesubmodules: + $(Q) (git submodule init) + $(Q) (git submodule update) + +MSG_DIR = $(PX4_BASE)msg +UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb +MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb +TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics +MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/$(PX4_TARGET_OS)/px4_messages +MULTIPLATFORM_PREFIX = px4_ +TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary +GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src +GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src + +.PHONY: generateuorbtopicheaders +generateuorbtopicheaders: checksubmodules + @$(ECHO) "Generating uORB topic headers" + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ + $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ + -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR)) + @$(ECHO) "Generating multiplatform uORB topic wrapper headers" + $(Q) (rm -r $(TOPICHEADER_TEMP_DIR)) + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ + $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ + -d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX)) + $(Q) (rm -r $(TOPICHEADER_TEMP_DIR)) + +# +# Testing targets +# +testbuild: + $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) + $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) + +nuttx posix qurt: +ifeq ($(GOALS),) + +$(MAKE) PX4_TARGET_OS=$@ $(GOALS) +else + export PX4_TARGET_OS=$@ +endif + +posixrun: + Tools/posix_run.sh + +qurtrun: + $(MAKE) PX4_TARGET_OS=qurt sim + +# +# Unittest targets. Builds and runs the host-level +# unit tests. +.PHONY: tests +tests: generateuorbtopicheaders + $(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests) + +.PHONY: format check_format +check_format: + $(Q) (./Tools/check_code_style.sh | sort -n) + +# +# Cleanup targets. 'clean' should remove all built products and force +# a complete re-compilation, 'distclean' should remove everything +# that's generated leaving only files that are in source control. +# +.PHONY: clean +clean: + @echo > /dev/null + $(Q) $(RMDIR) $(BUILD_DIR)*.build + $(Q) $(REMOVE) $(BUILD_DIR)git_version.* + $(Q) $(REMOVE) $(IMAGE_DIR)*.px4 + +.PHONY: distclean +distclean: clean + @echo > /dev/null + $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export + $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) + +# +# Print some help text +# +.PHONY: help +help: + @$(ECHO) "" + @$(ECHO) " PX4 firmware builder" + @$(ECHO) " ====================" + @$(ECHO) "" + @$(ECHO) " Available targets:" + @$(ECHO) " ------------------" + @$(ECHO) "" +ifeq ($(PX4_TARGET_OS),nuttx) + @$(ECHO) " archives" + @$(ECHO) " Build the NuttX RTOS archives that are used by the firmware build." + @$(ECHO) "" +endif + @$(ECHO) " all" + @$(ECHO) " Build all firmware configs: $(CONFIGS)" + @$(ECHO) " A limited set of configs can be built with CONFIGS=" + @$(ECHO) "" + @for config in $(CONFIGS); do \ + $(ECHO) " $$config"; \ + $(ECHO) " Build just the $$config firmware configuration."; \ + $(ECHO) ""; \ + done + @$(ECHO) " clean" + @$(ECHO) " Remove all firmware build pieces." + @$(ECHO) "" +ifeq ($(PX4_TARGET_OS),nuttx) + @$(ECHO) " distclean" + @$(ECHO) " Remove all compilation products, including NuttX RTOS archives." + @$(ECHO) "" + @$(ECHO) " upload" + @$(ECHO) " When exactly one config is being built, add this target to upload the" + @$(ECHO) " firmware to the board when the build is complete. Not supported for" + @$(ECHO) " all configurations." + @$(ECHO) "" +endif + @$(ECHO) " testbuild" + @$(ECHO) " Perform a complete clean build of the entire tree." + @$(ECHO) "" + @$(ECHO) " Common options:" + @$(ECHO) " ---------------" + @$(ECHO) "" + @$(ECHO) " V=1" + @$(ECHO) " If V is set, more verbose output is printed during the build. This can" + @$(ECHO) " help when diagnosing issues with the build or toolchain." + @$(ECHO) "" From d6b8e3a581b44070e712b9753c30799d834d1f57 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 08:47:02 +1100 Subject: [PATCH 265/293] added cmake Makefile wrapper from upstream --- Makefile | 452 ++++++++++++++++++------------------------------------- 1 file changed, 145 insertions(+), 307 deletions(-) diff --git a/Makefile b/Makefile index 081e912e62f8..cb7c7fbc8e8a 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,6 @@ +############################################################################ # -# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -28,344 +29,181 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # +############################################################################ +# Enforce the presence of the GIT repository # -# Top-level Makefile for building PX4 firmware images. -# - -TARGETS := nuttx posix qurt -EXPLICIT_TARGET := $(filter $(TARGETS),$(MAKECMDGOALS)) -ifneq ($(EXPLICIT_TARGET),) - export PX4_TARGET_OS=$(EXPLICIT_TARGET) - export GOALS := $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) +# We depend on our submodules, so we have to prevent attempts to +# compile without it being present. +ifeq ($(wildcard .git),) + $(error YOU HAVE TO USE GIT TO DOWNLOAD THIS REPOSITORY. ABORTING.) endif -# -# Get path and tool configuration -# -export PX4_BASE := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/ -include $(PX4_BASE)makefiles/setup.mk - -# -# Get a version string provided by git -# This assumes that git command is available and that -# the directory holding this file also contains .git directory -# -GIT_DESC := $(shell git log -1 --pretty=format:%H) -ifneq ($(words $(GIT_DESC)),1) - GIT_DESC := "unknown_git_version" +CMAKE_VER := $(shell Tools/check_cmake.sh; echo $$?) +ifneq ($(CMAKE_VER),0) + $(warning Not a valid CMake version or CMake not installed.) + $(warning On Ubuntu, install or upgrade via:) + $(warning ) + $(warning 3rd party PPA:) + $(warning sudo add-apt-repository ppa:george-edison55/cmake-3.x -y) + $(warning sudo apt-get update) + $(warning sudo apt-get install cmake) + $(warning ) + $(warning Official website:) + $(warning wget https://cmake.org/files/v3.3/cmake-3.3.2-Linux-x86_64.sh) + $(warning chmod +x cmake-3.3.2-Linux-x86_64.sh) + $(warning sudo mkdir /opt/cmake-3.3.2) + $(warning sudo ./cmake-3.3.2-Linux-x86_64.sh --prefix=/opt/cmake-3.3.2 --exclude-subdir) + $(warning export PATH=/opt/cmake-3.3.2/bin:$$PATH) + $(warning ) + $(error Fatal) endif -GIT_DESC_SHORT := $(shell echo $(GIT_DESC) | cut -c1-16) - -$(shell mkdir -p $(BUILD_DIR)) -$(shell rm -f $(BUILD_DIR)git_version.*) -$(shell echo "#include " > $(BUILD_DIR)git_version.c) -$(shell echo "const char* px4_git_version = \"$(GIT_DESC)\";" >> $(BUILD_DIR)git_version.c) -$(shell echo "const uint64_t px4_git_version_binary = 0x$(GIT_DESC_SHORT);" >> $(BUILD_DIR)git_version.c) - -# -# Canned firmware configurations that we (know how to) build. -# -KNOWN_CONFIGS := $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)/$(PX4_TARGET_OS)/config_*.mk)))) -CONFIGS ?= $(KNOWN_CONFIGS) - -# -# Boards that we (know how to) build NuttX export kits for. -# -KNOWN_BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)/$(PX4_TARGET_OS)/board_*.mk)))) -BOARDS ?= $(KNOWN_BOARDS) - -# -# Debugging -# -MQUIET = --no-print-directory -#MQUIET = --print-directory - -################################################################################ -# No user-serviceable parts below -################################################################################ - -# -# If the user has listed a config as a target, strip it out and override CONFIGS. -# -FIRMWARE_GOAL = firmware -EXPLICIT_CONFIGS := $(filter $(CONFIGS),$(MAKECMDGOALS)) -ifneq ($(EXPLICIT_CONFIGS),) -CONFIGS := $(EXPLICIT_CONFIGS) -.PHONY: $(EXPLICIT_CONFIGS) -$(EXPLICIT_CONFIGS): all +# Help +# -------------------------------------------------------------------- +# Don't be afraid of this makefile, it is just passing +# arguments to cmake to allow us to keep the wiki pages etc. +# that describe how to build the px4 firmware +# the same even when using cmake instead of make. +# +# Example usage: +# +# make px4fmu-v2_default (builds) +# make px4fmu-v2_default upload (builds and uploads) +# make px4fmu-v2_default test (builds and tests) +# +# This tells cmake to build the nuttx px4fmu-v2 default config in the +# directory build_nuttx_px4fmu-v2_default and then call make +# in that directory with the target upload. + +# explicity set default build target +all: px4fmu-v2_default + +# Parsing +# -------------------------------------------------------------------- +# assume 1st argument passed is the main target, the +# rest are arguments to pass to the makefile generated +# by cmake in the subdirectory +ARGS := $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) +j ?= 4 + +NINJA_BUILD := $(shell ninja --version 2>/dev/null) +ifdef NINJA_BUILD + PX4_CMAKE_GENERATOR ?= "Ninja" + PX4_MAKE = ninja + PX4_MAKE_ARGS = +else -# -# If the user has asked to upload, they must have also specified exactly one -# config. -# -ifneq ($(filter upload,$(MAKECMDGOALS)),) -ifneq ($(words $(EXPLICIT_CONFIGS)),1) -$(error In order to upload, exactly one board config must be specified) -endif -FIRMWARE_GOAL = upload -.PHONY: upload -upload: - @: +ifdef SYSTEMROOT + # Windows + PX4_CMAKE_GENERATOR ?= "MSYS Makefiles" +else + PX4_CMAKE_GENERATOR ?= "Unix Makefiles" endif + PX4_MAKE = make + PX4_MAKE_ARGS = -j$(j) --no-print-directory endif -ifeq ($(PX4_TARGET_OS),nuttx) -# -# Built products -# -DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) -STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) -FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) +# Functions +# -------------------------------------------------------------------- +# describe how to build a cmake config +define cmake-build ++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi ++@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi ++$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS) +endef -all: $(DESIRED_FIRMWARES) +# create empty targets to avoid msgs for targets passed to cmake +define cmake-targ +$(1): + @# +.PHONY: $(1) +endef -# -# Copy FIRMWARES into the image directory. -# -# XXX copying the .bin files is a hack to work around the PX4IO uploader -# not supporting .px4 files, and it should be deprecated onced that -# is taken care of. -# -$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 - @$(ECHO) %% Copying $@ - $(Q) $(COPY) $< $@ - $(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@) +# ADD CONFIGS HERE +# -------------------------------------------------------------------- +# Do not put any spaces between function arguments. -# -# Generate FIRMWARES. -# -.PHONY: $(FIRMWARES) -$(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) -$(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ -$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: generateuorbtopicheaders checksubmodules - @$(ECHO) %%%% - @$(ECHO) %%%% Building $(config) in $(work_dir) - @$(ECHO) %%%% - $(Q) $(MKDIR) -p $(work_dir) - $(Q)+ $(MAKE) -r -C $(work_dir) \ - -f $(PX4_MK_DIR)firmware.mk \ - CONFIG=$(config) \ - WORK_DIR=$(work_dir) \ - $(FIRMWARE_GOAL) +px4fmu-v1_default: + $(call cmake-build,nuttx_px4fmu-v1_default) -# -# Make FMU firmwares depend on the corresponding IO firmware. -# -# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency -# and forces the _default config in all cases. There has to be a better way to do this... -# -FMU_VERSION = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1)))) -define FMU_DEP -$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4 -endef -FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS)) -# $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) +px4fmu-v2_default: + $(call cmake-build,nuttx_px4fmu-v2_default) -# -# Build the NuttX export archives. -# -# Note that there are no explicit dependencies extended from these -# archives. If NuttX is updated, the user is expected to rebuild the -# archives/build area manually. Likewise, when the 'archives' target is -# invoked, all archives are always rebuilt. -# -# XXX Should support fetching/unpacking from a separate directory to permit -# downloads of the prebuilt archives as well... -# -NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) -.PHONY: archives -archives: checksubmodules $(NUTTX_ARCHIVES) - -$(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) -$(ARCHIVE_DIR)%.export: configuration = nsh -$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) - @$(ECHO) %% Configuring NuttX for $(board) - $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) - $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) - @$(ECHO) %% Exporting NuttX for $(board) - $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export - $(Q) $(MKDIR) -p $(dir $@) - $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ - $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) +px4fmu-v4_default: + $(call cmake-build,nuttx_px4fmu-v4_default) -# -# The user can run the NuttX 'menuconfig' tool for a single board configuration with -# make BOARDS= menuconfig -# -ifeq ($(MAKECMDGOALS),menuconfig) -ifneq ($(words $(BOARDS)),1) -$(error BOARDS must specify exactly one board for the menuconfig goal) -endif -BOARD = $(BOARDS) -menuconfig: $(NUTTX_SRC) - @$(ECHO) %% Configuring NuttX for $(BOARD) - $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) - $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) - @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig - @$(ECHO) %% Saving configuration file - $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig -else -menuconfig: - @$(ECHO) "" - @$(ECHO) "The menuconfig goal must be invoked without any other goal being specified" - @$(ECHO) "" +px4fmu-v2_simple: + $(call cmake-build,nuttx_px4fmu-v2_simple) -endif +px4fmu-v2_lpe: + $(call cmake-build,nuttx_px4fmu-v2_lpe) -$(NUTTX_SRC): checksubmodules +nuttx_sim_simple: + $(call cmake-build,$@) -$(UAVCAN_DIR): - $(Q) (./Tools/check_submodules.sh) +posix_sitl_simple: + $(call cmake-build,$@) -endif +posix_sitl_lpe: + $(call cmake-build,$@) -ifeq ($(PX4_TARGET_OS),nuttx) -# TODO -# Move the above nuttx specific rules into $(PX4_BASE)makefiles/firmware_nuttx.mk -endif -ifeq ($(PX4_TARGET_OS),posix) -include $(PX4_BASE)makefiles/firmware_posix.mk -endif -ifeq ($(PX4_TARGET_OS),qurt) -include $(PX4_BASE)makefiles/firmware_qurt.mk -endif +ros_sitl_simple: + $(call cmake-build,$@) +qurt_eagle_travis: + $(call cmake-build,$@) -.PHONY: checksubmodules -checksubmodules: - $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) - -.PHONY: updatesubmodules -updatesubmodules: - $(Q) (git submodule init) - $(Q) (git submodule update) - -MSG_DIR = $(PX4_BASE)msg -UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb -MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb -TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics -MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/$(PX4_TARGET_OS)/px4_messages -MULTIPLATFORM_PREFIX = px4_ -TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary -GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src -GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src - -.PHONY: generateuorbtopicheaders -generateuorbtopicheaders: checksubmodules - @$(ECHO) "Generating uORB topic headers" - $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ - $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ - -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR)) - @$(ECHO) "Generating multiplatform uORB topic wrapper headers" - $(Q) (rm -r $(TOPICHEADER_TEMP_DIR)) - $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ - $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ - -d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX)) - $(Q) (rm -r $(TOPICHEADER_TEMP_DIR)) +qurt_eagle_release: + $(call cmake-build,$@) -# -# Testing targets -# -testbuild: - $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) - $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) +posix_eagle_release: + $(call cmake-build,$@) -nuttx posix qurt: -ifeq ($(GOALS),) - +$(MAKE) PX4_TARGET_OS=$@ $(GOALS) -else - export PX4_TARGET_OS=$@ -endif +posix: posix_sitl_simple -posixrun: - Tools/posix_run.sh +posix_sitl_default: posix_sitl_simple -qurtrun: - $(MAKE) PX4_TARGET_OS=qurt sim +ros: ros_sitl_simple -# -# Unittest targets. Builds and runs the host-level -# unit tests. -.PHONY: tests -tests: generateuorbtopicheaders - $(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests) +sitl_deprecation: + @echo "Deprecated. Use 'make posix_sitl_default jmavsim' or" + @echo "'make posix_sitl_default gazebo' if Gazebo is preferred." -.PHONY: format check_format +sitl_quad: sitl_deprecation +sitl_plane: sitl_deprecation +sitl_ros: sitl_deprecation + +# Other targets +# -------------------------------------------------------------------- check_format: - $(Q) (./Tools/check_code_style.sh | sort -n) + @./Tools/check_code_style.sh -# -# Cleanup targets. 'clean' should remove all built products and force -# a complete re-compilation, 'distclean' should remove everything -# that's generated leaving only files that are in source control. -# -.PHONY: clean clean: - @echo > /dev/null - $(Q) $(RMDIR) $(BUILD_DIR)*.build - $(Q) $(REMOVE) $(BUILD_DIR)git_version.* - $(Q) $(REMOVE) $(IMAGE_DIR)*.px4 - -.PHONY: distclean -distclean: clean - @echo > /dev/null - $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export - $(Q)+ $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) + @rm -rf build_*/ + @(cd NuttX && git clean -d -f -x) + @(cd src/modules/uavcan/libuavcan && git clean -d -f -x) + +# targets handled by cmake +cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ + run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \ + jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_tailsitter +$(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) +.PHONY: clean + +CONFIGS:=$(shell ls cmake/configs | sed -e "s~.*/~~" | sed -e "s~\..*~~") + +# Future: +#$(CONFIGS): +## @cd Build/$@ && cmake ../.. -DCONFIG=$@ +# @cd Build/$@ && make # -# Print some help text +#clean-all: +# @rm -rf Build/* # -.PHONY: help -help: - @$(ECHO) "" - @$(ECHO) " PX4 firmware builder" - @$(ECHO) " ====================" - @$(ECHO) "" - @$(ECHO) " Available targets:" - @$(ECHO) " ------------------" - @$(ECHO) "" -ifeq ($(PX4_TARGET_OS),nuttx) - @$(ECHO) " archives" - @$(ECHO) " Build the NuttX RTOS archives that are used by the firmware build." - @$(ECHO) "" -endif - @$(ECHO) " all" - @$(ECHO) " Build all firmware configs: $(CONFIGS)" - @$(ECHO) " A limited set of configs can be built with CONFIGS=" - @$(ECHO) "" - @for config in $(CONFIGS); do \ - $(ECHO) " $$config"; \ - $(ECHO) " Build just the $$config firmware configuration."; \ - $(ECHO) ""; \ - done - @$(ECHO) " clean" - @$(ECHO) " Remove all firmware build pieces." - @$(ECHO) "" -ifeq ($(PX4_TARGET_OS),nuttx) - @$(ECHO) " distclean" - @$(ECHO) " Remove all compilation products, including NuttX RTOS archives." - @$(ECHO) "" - @$(ECHO) " upload" - @$(ECHO) " When exactly one config is being built, add this target to upload the" - @$(ECHO) " firmware to the board when the build is complete. Not supported for" - @$(ECHO) " all configurations." - @$(ECHO) "" -endif - @$(ECHO) " testbuild" - @$(ECHO) " Perform a complete clean build of the entire tree." - @$(ECHO) "" - @$(ECHO) " Common options:" - @$(ECHO) " ---------------" - @$(ECHO) "" - @$(ECHO) " V=1" - @$(ECHO) " If V is set, more verbose output is printed during the build. This can" - @$(ECHO) " help when diagnosing issues with the build or toolchain." - @$(ECHO) "" +#help: +# @echo +# @echo "Type 'make ' and hit the tab key twice to see a list of the available" +# @echo "build configurations." +# @echo From 0507f6b36c34220b029c8a21b6b89fc6239db12d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 08:51:50 +1100 Subject: [PATCH 266/293] merge: added missing files from upstream --- Documentation/px4_hil/SITL_Diagram.png | Bin 0 -> 80757 bytes Documentation/px4_hil/SITL_Diagram_QGC.png | Bin 0 -> 75957 bytes Documentation/px4_hil/UserGuide.md | 98 + Documentation/px4_hil/docs/readme.txt | 0 Documentation/px4_hil/px4_hil.doxyfile | 2403 +++++++++++++++++ ROMFS/px4fmu_common/init.d/10020_3dr_quad | 30 + .../init.d/13003_quad_tailsitter | 21 + .../init.d/13004_quad+_tailsitter | 21 + .../init.d/13005_vtol_AAERT_quad | 50 + .../init.d/13006_vtol_standard_delta | 50 + ROMFS/px4fmu_common/init.d/2101_fw_AERT | 28 + ROMFS/px4fmu_common/init.d/2104_fw_AETR | 28 + ROMFS/px4fmu_common/init.d/2105_maja | 50 + ROMFS/px4fmu_common/init.d/2106_albatross | 51 + ROMFS/px4fmu_common/init.d/4009_qav250 | 33 + ROMFS/px4fmu_common/mixers/AAERTWF.main.mix | 96 + ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix | 84 + ROMFS/px4fmu_common/mixers/AETR.main.mix | 84 + ROMFS/px4fmu_common/mixers/caipi.main.mix | 51 + .../px4fmu_common/mixers/quad_+_vtol.main.mix | 28 + .../px4fmu_common/mixers/quad_x_vtol.main.mix | 18 + ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix | 32 + ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix | 39 + .../px4fmu_common/mixers/vtol_quad_x.main.mix | 4 + ROMFS/px4fmu_test/mixers/IO_pass.mix | 24 + Tools/Matlab/ellipsoid_fit.m | 174 ++ Tools/Matlab/plot_mag.m | 77 + Tools/airframes.xml | 13 + Tools/astylerc | 18 + Tools/check_cmake.sh | 14 + Tools/px4airframes/README.md | 1 + Tools/px4airframes/__init__.py | 1 + Tools/px4airframes/rcout.py | 55 + Tools/px4airframes/srcparser.py | 347 +++ Tools/px4airframes/srcscanner.py | 38 + Tools/px4airframes/xmlout.py | 88 + Tools/px_generate_params.py | 100 + Tools/px_process_airframes.py | 110 + Tools/sitl_run.sh | 103 + Vagrantfile | 106 + cmake/common/px4_base.cmake | 871 ++++++ cmake/configs/nuttx_px4fmu-v1_default.cmake | 179 ++ cmake/configs/nuttx_px4fmu-v2_default.cmake | 188 ++ cmake/configs/nuttx_px4fmu-v2_lpe.cmake | 5 + cmake/configs/nuttx_px4fmu-v2_simple.cmake | 143 + cmake/configs/nuttx_sim_simple.cmake | 21 + cmake/configs/posix_eagle_default.cmake | 48 + cmake/configs/posix_eagle_hil.cmake | 40 + cmake/configs/posix_eagle_muorb.cmake | 15 + cmake/configs/posix_eagle_release.cmake | 44 + cmake/configs/posix_sitl_lpe.cmake | 9 + cmake/configs/posix_sitl_simple.cmake | 95 + cmake/configs/qurt_eagle_hello.cmake | 28 + cmake/configs/qurt_eagle_hil.cmake | 65 + cmake/configs/qurt_eagle_muorb.cmake | 41 + cmake/configs/qurt_eagle_release.cmake | 87 + cmake/configs/qurt_eagle_test.cmake | 39 + cmake/configs/qurt_eagle_travis.cmake | 70 + cmake/nuttx/bin_to_obj.py | 98 + cmake/nuttx/builtin_commands.c.in | 10 + cmake/nuttx/px4_impl_nuttx.cmake | 537 ++++ cmake/posix/apps.h_in | 88 + cmake/posix/ld.script | 47 + cmake/posix/px4_impl_posix.cmake | 236 ++ cmake/qurt/apps.h_in | 84 + cmake/qurt/px4_impl_qurt.cmake | 229 ++ cmake/qurt/qurt_eigen.patch | 37 + cmake/qurt/qurt_funcs.cmake | 48 + cmake/ros-CMakeLists.txt | 335 +++ cmake/scripts/convert_modules_to_cmake.py | 91 + cmake/scripts/test_compare.py | 50 + cmake/templates/build_git_version.h.in | 4 + cmake/templates/cmake_lists.jinja | 73 + cmake/test/cmake_tester.py | 52 + cmake/test/px4_simple_app_correct.txt | 27 + cmake/test/px4_simple_app_input.txt | 4 + .../Toolchain-arm-linux-gnueabihf.cmake | 67 + .../toolchains/Toolchain-arm-none-eabi.cmake | 85 + .../toolchains/Toolchain-hexagon-7.2.10.cmake | 254 ++ cmake/toolchains/Toolchain-hexagon.cmake | 249 ++ cmake/toolchains/Toolchain-native.cmake | 17 + launch/gazebo_iris_mavros_posix_sitl.launch | 23 + launch/multi_uav.launch | 55 + nuttx-configs/sim/include/README.txt | 1 + nuttx-configs/sim/include/board.h | 0 nuttx-configs/sim/nsh/Make.defs | 111 + nuttx-configs/sim/nsh/defconfig | 531 ++++ nuttx-configs/sim/nsh/setenv.sh | 45 + posix-configs/SITL/README.md | 109 + posix-configs/SITL/SITL_Diagram.png | Bin 0 -> 80757 bytes posix-configs/SITL/init/rc.fixed_wing | 49 + posix-configs/SITL/init/rcS_gazebo_iris | 59 + posix-configs/SITL/init/rcS_gazebo_tailsitter | 64 + posix-configs/SITL/init/rcS_jmavsim_iris | 58 + posix-configs/SITL/init/rcS_lpe_gazebo | 66 + posix-configs/SITL/init/rcS_lpe_jmavsim | 65 + posix-configs/SITL/init/rc_iris_ros | 70 + src/drivers/airspeed/CMakeLists.txt | 42 + src/drivers/ardrone_interface/CMakeLists.txt | 45 + src/drivers/batt_smbus/CMakeLists.txt | 43 + src/drivers/blinkm/CMakeLists.txt | 43 + src/drivers/bma180/CMakeLists.txt | 43 + src/drivers/boards/aerocore/CMakeLists.txt | 45 + src/drivers/boards/px4io-v2/CMakeLists.txt | 43 + src/drivers/boards/sim/board_config.h | 13 + src/drivers/boards/sitl/sitl_led.c | 81 + src/drivers/camera_trigger/camera_trigger.cpp | 419 +++ .../camera_trigger/camera_trigger_params.c | 97 + src/drivers/l3gd20/CMakeLists.txt | 45 + src/drivers/oreoled/CMakeLists.txt | 43 + src/drivers/pwm_input/CMakeLists.txt | 44 + src/drivers/pwm_out_sim/pwm_out_sim.cpp | 1037 +++++++ src/drivers/px4io/CMakeLists.txt | 48 + src/drivers/px4io/px4io_driver.h | 50 + src/drivers/rgbled/CMakeLists.txt | 43 + src/drivers/srf02/srf02.cpp | 959 +++++++ src/drivers/stm32/CMakeLists.txt | 43 + src/examples/fixedwing_control/CMakeLists.txt | 46 + src/examples/hwtest/CMakeLists.txt | 41 + src/examples/matlab_csv_serial/CMakeLists.txt | 41 + src/examples/publisher/CMakeLists.txt | 44 + src/examples/px4_daemon_app/CMakeLists.txt | 42 + src/examples/px4_mavlink_debug/CMakeLists.txt | 42 + src/examples/px4_simple_app/CMakeLists.txt | 41 + .../rover_steering_control/CMakeLists.txt | 46 + src/examples/subscriber/CMakeLists.txt | 45 + src/firmware/nuttx/gdbinit.in | 7 + src/lib/conversion/CMakeLists.txt | 42 + src/lib/ecl/CMakeLists.txt | 49 + .../ecl/attitude_fw/ecl_wheel_controller.cpp | 153 ++ .../ecl/attitude_fw/ecl_wheel_controller.h | 70 + src/lib/ecl/validation/data_validator.cpp | 169 ++ src/lib/ecl/validation/data_validator.h | 159 ++ .../ecl/validation/data_validator_group.cpp | 266 ++ src/lib/ecl/validation/data_validator_group.h | 122 + src/lib/external_lgpl/CMakeLists.txt | 42 + src/lib/geo/CMakeLists.txt | 40 + src/lib/geo_lookup/CMakeLists.txt | 42 + src/lib/launchdetection/CMakeLists.txt | 43 + src/lib/mathlib/CMakeLists.txt | 41 + src/lib/mathlib/math/filter/CMakeLists.txt | 40 + src/lib/runway_takeoff/RunwayTakeoff.cpp | 286 ++ src/lib/runway_takeoff/RunwayTakeoff.h | 121 + .../runway_takeoff/runway_takeoff_params.c | 137 + .../terrain_estimation/terrain_estimator.cpp | 201 ++ .../terrain_estimation/terrain_estimator.h | 100 + src/systemcmds/bl_update/CMakeLists.txt | 44 + src/systemcmds/config/CMakeLists.txt | 44 + src/systemcmds/dumpfile/CMakeLists.txt | 43 + src/systemcmds/esc_calib/CMakeLists.txt | 44 + src/systemcmds/i2c/CMakeLists.txt | 43 + src/systemcmds/mixer/CMakeLists.txt | 50 + src/systemcmds/motor_test/CMakeLists.txt | 44 + src/systemcmds/mtd/CMakeLists.txt | 45 + src/systemcmds/nshterm/CMakeLists.txt | 45 + src/systemcmds/param/CMakeLists.txt | 44 + src/systemcmds/perf/CMakeLists.txt | 44 + src/systemcmds/pwm/CMakeLists.txt | 44 + src/systemcmds/reboot/CMakeLists.txt | 44 + src/systemcmds/reflect/CMakeLists.txt | 43 + src/systemcmds/tests/CMakeLists.txt | 81 + src/systemcmds/tests/test_params.c | 106 + src/systemcmds/top/CMakeLists.txt | 44 + src/systemcmds/topic_listener/CMakeLists.txt | 55 + src/systemcmds/usb_connected/CMakeLists.txt | 43 + src/systemcmds/ver/CMakeLists.txt | 44 + template_.cproject | 178 ++ template_.project | 38 + 168 files changed, 17538 insertions(+) create mode 100644 Documentation/px4_hil/SITL_Diagram.png create mode 100644 Documentation/px4_hil/SITL_Diagram_QGC.png create mode 100644 Documentation/px4_hil/UserGuide.md create mode 100644 Documentation/px4_hil/docs/readme.txt create mode 100644 Documentation/px4_hil/px4_hil.doxyfile create mode 100644 ROMFS/px4fmu_common/init.d/10020_3dr_quad create mode 100644 ROMFS/px4fmu_common/init.d/13003_quad_tailsitter create mode 100644 ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter create mode 100644 ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad create mode 100644 ROMFS/px4fmu_common/init.d/13006_vtol_standard_delta create mode 100644 ROMFS/px4fmu_common/init.d/2101_fw_AERT create mode 100644 ROMFS/px4fmu_common/init.d/2104_fw_AETR create mode 100644 ROMFS/px4fmu_common/init.d/2105_maja create mode 100644 ROMFS/px4fmu_common/init.d/2106_albatross create mode 100644 ROMFS/px4fmu_common/init.d/4009_qav250 create mode 100644 ROMFS/px4fmu_common/mixers/AAERTWF.main.mix create mode 100644 ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix create mode 100644 ROMFS/px4fmu_common/mixers/AETR.main.mix create mode 100644 ROMFS/px4fmu_common/mixers/caipi.main.mix create mode 100644 ROMFS/px4fmu_common/mixers/quad_+_vtol.main.mix create mode 100644 ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix create mode 100644 ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix create mode 100644 ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix create mode 100644 ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix create mode 100644 ROMFS/px4fmu_test/mixers/IO_pass.mix create mode 100644 Tools/Matlab/ellipsoid_fit.m create mode 100644 Tools/Matlab/plot_mag.m create mode 100644 Tools/airframes.xml create mode 100644 Tools/astylerc create mode 100755 Tools/check_cmake.sh create mode 100644 Tools/px4airframes/README.md create mode 100644 Tools/px4airframes/__init__.py create mode 100644 Tools/px4airframes/rcout.py create mode 100644 Tools/px4airframes/srcparser.py create mode 100644 Tools/px4airframes/srcscanner.py create mode 100644 Tools/px4airframes/xmlout.py create mode 100755 Tools/px_generate_params.py create mode 100755 Tools/px_process_airframes.py create mode 100755 Tools/sitl_run.sh create mode 100644 Vagrantfile create mode 100644 cmake/common/px4_base.cmake create mode 100644 cmake/configs/nuttx_px4fmu-v1_default.cmake create mode 100644 cmake/configs/nuttx_px4fmu-v2_default.cmake create mode 100644 cmake/configs/nuttx_px4fmu-v2_lpe.cmake create mode 100644 cmake/configs/nuttx_px4fmu-v2_simple.cmake create mode 100644 cmake/configs/nuttx_sim_simple.cmake create mode 100644 cmake/configs/posix_eagle_default.cmake create mode 100644 cmake/configs/posix_eagle_hil.cmake create mode 100644 cmake/configs/posix_eagle_muorb.cmake create mode 100644 cmake/configs/posix_eagle_release.cmake create mode 100644 cmake/configs/posix_sitl_lpe.cmake create mode 100644 cmake/configs/posix_sitl_simple.cmake create mode 100644 cmake/configs/qurt_eagle_hello.cmake create mode 100644 cmake/configs/qurt_eagle_hil.cmake create mode 100644 cmake/configs/qurt_eagle_muorb.cmake create mode 100644 cmake/configs/qurt_eagle_release.cmake create mode 100644 cmake/configs/qurt_eagle_test.cmake create mode 100644 cmake/configs/qurt_eagle_travis.cmake create mode 100755 cmake/nuttx/bin_to_obj.py create mode 100644 cmake/nuttx/builtin_commands.c.in create mode 100644 cmake/nuttx/px4_impl_nuttx.cmake create mode 100644 cmake/posix/apps.h_in create mode 100644 cmake/posix/ld.script create mode 100644 cmake/posix/px4_impl_posix.cmake create mode 100644 cmake/qurt/apps.h_in create mode 100644 cmake/qurt/px4_impl_qurt.cmake create mode 100644 cmake/qurt/qurt_eigen.patch create mode 100644 cmake/qurt/qurt_funcs.cmake create mode 100644 cmake/ros-CMakeLists.txt create mode 100755 cmake/scripts/convert_modules_to_cmake.py create mode 100755 cmake/scripts/test_compare.py create mode 100644 cmake/templates/build_git_version.h.in create mode 100644 cmake/templates/cmake_lists.jinja create mode 100755 cmake/test/cmake_tester.py create mode 100644 cmake/test/px4_simple_app_correct.txt create mode 100644 cmake/test/px4_simple_app_input.txt create mode 100644 cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake create mode 100644 cmake/toolchains/Toolchain-arm-none-eabi.cmake create mode 100644 cmake/toolchains/Toolchain-hexagon-7.2.10.cmake create mode 100644 cmake/toolchains/Toolchain-hexagon.cmake create mode 100644 cmake/toolchains/Toolchain-native.cmake create mode 100644 launch/gazebo_iris_mavros_posix_sitl.launch create mode 100644 launch/multi_uav.launch create mode 100644 nuttx-configs/sim/include/README.txt create mode 100644 nuttx-configs/sim/include/board.h create mode 100644 nuttx-configs/sim/nsh/Make.defs create mode 100644 nuttx-configs/sim/nsh/defconfig create mode 100755 nuttx-configs/sim/nsh/setenv.sh create mode 100644 posix-configs/SITL/README.md create mode 100644 posix-configs/SITL/SITL_Diagram.png create mode 100644 posix-configs/SITL/init/rc.fixed_wing create mode 100644 posix-configs/SITL/init/rcS_gazebo_iris create mode 100644 posix-configs/SITL/init/rcS_gazebo_tailsitter create mode 100644 posix-configs/SITL/init/rcS_jmavsim_iris create mode 100644 posix-configs/SITL/init/rcS_lpe_gazebo create mode 100644 posix-configs/SITL/init/rcS_lpe_jmavsim create mode 100644 posix-configs/SITL/init/rc_iris_ros create mode 100644 src/drivers/airspeed/CMakeLists.txt create mode 100644 src/drivers/ardrone_interface/CMakeLists.txt create mode 100644 src/drivers/batt_smbus/CMakeLists.txt create mode 100644 src/drivers/blinkm/CMakeLists.txt create mode 100644 src/drivers/bma180/CMakeLists.txt create mode 100644 src/drivers/boards/aerocore/CMakeLists.txt create mode 100644 src/drivers/boards/px4io-v2/CMakeLists.txt create mode 100644 src/drivers/boards/sim/board_config.h create mode 100644 src/drivers/boards/sitl/sitl_led.c create mode 100644 src/drivers/camera_trigger/camera_trigger.cpp create mode 100644 src/drivers/camera_trigger/camera_trigger_params.c create mode 100644 src/drivers/l3gd20/CMakeLists.txt create mode 100644 src/drivers/oreoled/CMakeLists.txt create mode 100644 src/drivers/pwm_input/CMakeLists.txt create mode 100644 src/drivers/pwm_out_sim/pwm_out_sim.cpp create mode 100644 src/drivers/px4io/CMakeLists.txt create mode 100644 src/drivers/px4io/px4io_driver.h create mode 100644 src/drivers/rgbled/CMakeLists.txt create mode 100644 src/drivers/srf02/srf02.cpp create mode 100644 src/drivers/stm32/CMakeLists.txt create mode 100644 src/examples/fixedwing_control/CMakeLists.txt create mode 100644 src/examples/hwtest/CMakeLists.txt create mode 100644 src/examples/matlab_csv_serial/CMakeLists.txt create mode 100644 src/examples/publisher/CMakeLists.txt create mode 100644 src/examples/px4_daemon_app/CMakeLists.txt create mode 100644 src/examples/px4_mavlink_debug/CMakeLists.txt create mode 100644 src/examples/px4_simple_app/CMakeLists.txt create mode 100644 src/examples/rover_steering_control/CMakeLists.txt create mode 100644 src/examples/subscriber/CMakeLists.txt create mode 100644 src/firmware/nuttx/gdbinit.in create mode 100644 src/lib/conversion/CMakeLists.txt create mode 100644 src/lib/ecl/CMakeLists.txt create mode 100644 src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp create mode 100644 src/lib/ecl/attitude_fw/ecl_wheel_controller.h create mode 100644 src/lib/ecl/validation/data_validator.cpp create mode 100644 src/lib/ecl/validation/data_validator.h create mode 100644 src/lib/ecl/validation/data_validator_group.cpp create mode 100644 src/lib/ecl/validation/data_validator_group.h create mode 100644 src/lib/external_lgpl/CMakeLists.txt create mode 100644 src/lib/geo/CMakeLists.txt create mode 100644 src/lib/geo_lookup/CMakeLists.txt create mode 100644 src/lib/launchdetection/CMakeLists.txt create mode 100644 src/lib/mathlib/CMakeLists.txt create mode 100644 src/lib/mathlib/math/filter/CMakeLists.txt create mode 100644 src/lib/runway_takeoff/RunwayTakeoff.cpp create mode 100644 src/lib/runway_takeoff/RunwayTakeoff.h create mode 100644 src/lib/runway_takeoff/runway_takeoff_params.c create mode 100644 src/lib/terrain_estimation/terrain_estimator.cpp create mode 100644 src/lib/terrain_estimation/terrain_estimator.h create mode 100644 src/systemcmds/bl_update/CMakeLists.txt create mode 100644 src/systemcmds/config/CMakeLists.txt create mode 100644 src/systemcmds/dumpfile/CMakeLists.txt create mode 100644 src/systemcmds/esc_calib/CMakeLists.txt create mode 100644 src/systemcmds/i2c/CMakeLists.txt create mode 100644 src/systemcmds/mixer/CMakeLists.txt create mode 100644 src/systemcmds/motor_test/CMakeLists.txt create mode 100644 src/systemcmds/mtd/CMakeLists.txt create mode 100644 src/systemcmds/nshterm/CMakeLists.txt create mode 100644 src/systemcmds/param/CMakeLists.txt create mode 100644 src/systemcmds/perf/CMakeLists.txt create mode 100644 src/systemcmds/pwm/CMakeLists.txt create mode 100644 src/systemcmds/reboot/CMakeLists.txt create mode 100644 src/systemcmds/reflect/CMakeLists.txt create mode 100644 src/systemcmds/tests/CMakeLists.txt create mode 100644 src/systemcmds/tests/test_params.c create mode 100644 src/systemcmds/top/CMakeLists.txt create mode 100644 src/systemcmds/topic_listener/CMakeLists.txt create mode 100644 src/systemcmds/usb_connected/CMakeLists.txt create mode 100644 src/systemcmds/ver/CMakeLists.txt create mode 100644 template_.cproject create mode 100644 template_.project diff --git a/Documentation/px4_hil/SITL_Diagram.png b/Documentation/px4_hil/SITL_Diagram.png new file mode 100644 index 0000000000000000000000000000000000000000..bbd2dbcceee688c356722ca5a86930689fbf0da6 GIT binary patch literal 80757 zcmeEuWmuHo`mUgewg9u0wmf=VO#{=qI=>s%II>UYA}VNoAi>!;Rw z(ps5O#Gi;h)z?=^zIVph-QC4I82_CaIlu9-l&ABcaR?+`KV)q++Y9oOFC3+(CaAmfWNY%nW7@5`Zpr z_iuCkGlDk_N&vc;42$?bE(4PTf*|{|qQB1r8-oKv`tBXg-~a#9E!=0|FSC83FX;_$ z#UsE{_{(GfncV;DJj4K*1jMNa{Y57F&_%Alh!c|(Umwcx;9rVNNKnPeJL-pOA@wz@2AG1!OpNnuKv^w$Xr$o8Iu~_*qyc@w z{4NgNn8=_@2){LeS#kZYE~zb9Qt)i1hzkW_s<#ASZct^1LzAa~LGbAUJ?&e!kRu(T zR2le3k>4}ywj~Q-Af-W}R7j^;Bm|5~b_l!h9?~@i9_m98iPdXKir%QZj4g;oPwO6m z+t)^sugyUUDS_8~%z$~FdEa`Nw+`_6S5IFE(s)br0dG{{cj3G>_(1}F=pez;-tBBk zfG3C5IF`5TVg=gFmTR~u0h11V08Cm^?2rd|ZGd=Cck)q|ew`i&=9{7EGm!<%5YGW6 zATW;N02?WvMC7VA6M~2`k#olfCdD=+Q}`G;sW=QXIE^&T@3^F{pK^M9?k_z)3W7}9 z%J`5AV04Jwd zIvBkj_Z84YL-XS?(!m#}fpLrHJPQSp9sI>B9f3G=7KD@x;ttRpWM(u^i+o7{Smh*2 zeC%y=GvJ9t&7BfCq>crF7PA?TTu6|^qyv*#yeoR^sN;BmmI7!8M38TFBQ2r9M_mvp zSvFc=nzT6W7J!xrgM#nC#AJ@nH2%mOIX=t(jq#7##P)xY^)>l6nzCAM=F2pL zABpu^IAJ96>Yg65{E0&S^uXk_cUq^BlPC@L)`hB1HHS=1;E-niJ{Umg^Dqk|U;L|W zk3Ss50^MkE#d)ONT`$@241kNo1|j8!`8^6S9BFP6W@%(2PLxK3e!oWKW@5gXx6Z;5#KUAga5;YKs^SHUoKnqO0VpO(0iKVdAA4gdmNcg9@tvdC^^Lo<3-poU$%!>NIBa*1xzZ| zf=x9BDFlcPC4cvTCcz)evI{SfoPxU;Fp2|Pa)zvL_GJKZ1Q%#Tces>UsuvihtpGj50)b(Idh&Y@yK)uW` zqt98meeu`$<%X{oZ<)z36ngnsJSn`6wkaHRQgn^wGgBk(@^A{PMZiGxv0MV~6y_zJ zLldgO3HpQIBGSp>Giq#xJEOspi4e5;y-Bq9jg3XlBQ~B44sE{j(DtxUWhwXf%dLj= zbSUYXt=d7`tNt0Wwz)*HALkoKR1E*w?p9w!dpMrr@(iThb-{=h(`8w{yr8X)B}o?c z{JiL;_hMc;>3+0UrD*!;SN>mXF`?&=g1^#>MH3fU>!PbY0F~%Aw0shN4R0UGd9X1| zH2m5g2%oxFXNb_uM%`xjVAF7LK(%$itc!e^d9NhQKqfg{$<5htqQ{uYUDaZGc+Gl^ zjf8_2qQ2ck1}>!UKD@Ojm2~D$R}lUkLH^^L#KSbUS1^Dl7er>78>-~*uVlG)445Z zyrwYItkeC`%knP_W+Z3cOrMO-3-XoT*pBroXIkJ87Ro=B(XfP2ZJWxpS5@RFREroI zvH!9?UGE@b&m{q0RhYEwNXU)a7arKsb^08tIfgv@XUn(0>FJ5vh+ghoyb0Zw{Kmv> zQN5iR@buLOh}+czFS+E@fE}ZLJv^d>=QWo&^L367z24ps9I)1YNe-;BozI<*oGhbT za8ld5PdRi*-3Z|n?_!pvY}^t#1QgD{<#fp+Q{wXlk*c$}D7rc&R#Fg5huPnR{^A=fl`j_+8<#$5; z{S#NiCu^5+?hDJ?X)|VzonK_ftTms*PkX;`$!L(FIyi2d%<_2=%TWo5T9r&9q!RCK zI;@B{m9NqrDYKj%n~p^eeLp3EULa_~tfu_jCQ9ool2?i?{6sIpp8ltFVVvYc^M(I zP3}^C@evtgHNt-1v<}ZFpL_AJp@Dt9jy0di?>2++8{LjZ7qZx@rZpj! zeQZU2hyH4}iW%y@mrTB;T%prBxPNA!Mo;IZ(#)({!Gmi%?Oli#LT#QO1wGk#)(@V| zVX!h~UkWd$daio>qTHCyMwhQdH)b{A1iSVyFXd9LsMwW+`1cKS@@#Y9Qq`9Jo?bx& zr^kJ0duTz&e($}d*nyOS?t0D=F;_K3ldjAr3@^uZp3ECglsawSjxNnoyIfy>IJ|Fr zLY>!xb630eGjyMT+Irk7h=4u$n>*V?gjdM>9_-(Z8`gT&q^6)>V(^|ZJF81#qEdW@ zx1X+zdkb0~&h4+A4s6+qwc_yf~TPABQU^Z;;wRX04&iplDsJ-|2dm ztS~|7RbR82(Qv)NzS#O2)hHHddZ!cEay68452Y6c~sc3tzJwC3x&`Vc^p%I-pz?j6J`SN86puc4bN@Z!?yKPS(0( z$nGsrq=gplyPNSoc3@h=dGsJY*E%z{m%}fC_&GWF@fz#q1X(l7oc9jnK&QGdoTkGM zf`Z&}bC5f3{+iu&&(AZs7nJ0PI6A>A=(CGxQ#bGFclRYep8lk~ras%*96i4=*A

35|u#2;t=pUN;ye$-)vvxKw!tuP)#;EAE@+>+XoitObQ^|1<}>D zqKId5y2pmBkS_de{I8#XIt=#Ln9s9tUol;Yo9~>yZu7m@JOXS61K*;z4B?E=wU*C0 ztOKh?7?(S0E56`XaGusWoF2S}`SNw`4)KB^ZE@UMhCka7c!r|Mx0#v}kg4&hIDG$L zZ1isT2xTERA$LCe?yzyM%@9sE9!9H3-y?p9f&uY`Lp*%&r)E8L7wKpmrO;l+aKq)q_z2sxgp+Hg;TJtMX9w1v z*$wCNc~gP;pN9~3tD{m^TyAyb_M_h?NWP@}Uhp6}h+^@@uez&D#tM;)i;y?yFEbjo zgEYBG9NYWt8Gkthwnm$)Gs&-zSAI&;B@sU_BZvxDBKWB|SEL0BkKN;Rp?eDj@7+6j zR@gMi=`AO?*FOjr7xKf?^s^n!bc=I2r#;qa!4Qv!o5{GZT^SxjB)i!3m{#1k_ViCt zj#K8oky=)~bFohqZ~E8~HVD$L3QK}Qj}u?)Tm zKg^H6!c{Z}F}!dvcsPI4=$s_x&etVfR1t&iiP-%WQI3Beq#Tc&Mr5gGY;6wCJeIAG z^J}8KX^&u#9)B=V({cDvL%^sji9|&+jwI%1+k4-(H9vH%!fHZ-w&KvcZTCW-VX5l> zYKxd>b-=~IR|z3`ef=p5=J(K{zx6hehv)!dVUde!7{oOia!Od%d*iC;Dt9o6ZHrvMvuj}OLwK~1y$!f;2k4&CCJXe0+r*B4=Xh1sT8G8 zH?U?81U{|gF_?Bd`!+^p#>=0^4{zmd;`OvrK<6LM{!zcEFB!4H<%A)t(;S-Y#FQpE zestzOtvKs)rL5K&5A!_|cNhG?bWvqZ{vuEFG~mxsIS>m_;@MsOkeS$PGF7=VG3y>; zc29TieSXU{TFJ%j0=CnWK8h+i82s(;#r6?K3)z=NCrM(bx)CZaEA2YEMzE4eI%yvp z4G#43!~#7aCYS4v!t5ui)1O!{mJLGWa6ShD-PA#Sh z!9^g=nEOjB0nQQ5mZ8ou{S9qosm;bXif_L6sU2QzCi$+838 zWnJlY8=+}!{_Yi2M<+$o*5^|hfff>Gn`4r3d{P^oN4h6;TGmnwXKYjgib-Mt(@;i) z!Xb-MvCsT@rcF7l5BD{}Qo zZ-O=tY1Si6&2|TUEJ*Ba@N<5fS^=l~`3&wgo0;%2^f0>OSdtj`si)5&UD5d@J^m-r z(rK$f=5-_=))3T!Aa0=@kcPkcMAXB&)3#d#@yQ(k?0#Ar@7*BYlWiCqMx`SCsi(t& zb5ycPL@u5tV;W_OU1D^*xVJRhaE533qP7BhbM}S&8=U}C@0!Cf-9sU}jmBRRRZ*6U zCq;bKhRnLnP!u5jAG>*QVBI`_es32@V}XsSzD{oQ=@(sssY@E2YDx>k?}&vuAjqy< z;m55cTwzIo3UJ^?AT+Z6=r(rWi)_ILJwOZ%gX};8J55x-xqb~h%9PXrB=WRit^6}Z z(fBF5>D17FF6d2jY=+v1BZaxC5EzSA`{g$%fASW&CghjpqIhVM{dBlG)GCA%s1@ z0#W)bmwjmn99Up8FY^{^mWuT^-I<=f;h0k)Xdp^#nduBMk74bB8B7B`Eg9 zth=K;3F`x`?JXHkG#iNzVha{Woty%C;t9anPPYE9>TwOv=QF+p1QM@)N_S10-QX~Z z*)8ICgPL7lb5y}8VZJs><+sobeM>UB&;!EMm$@x{AtZ>?C=B38=|7zS9QR|DwjgzG zB)zXe5)Jo+%qA=3{B)n>6ju5&1KH_@Vg^e-VOQ`o%WD0-sb2NL8ej5iM*w0_W{$(! z$&dOmEGWU@0%M!2w?9Fo_PH(2gX-RP-)a~ze&iL(Lo^=txR8*}u++0lr-&M5jhx5R z9F{MNlZAqTbByQ-?W?5nuzuC2e;~7ckBaH8SemyR7SgsKKPt2PQTnLaEY{Z+>>u=L z&CNIhgY2k2@+7gGqcFS`9GD4M8ORzs&x4kZA3>C;`k5j$b|9HHwKoSifJZACB+WO2 z1Y+4=12}|2)+^?m`~)twjzk3YDS&%jLe5P4yjOzp^R*(gG(gGCA9ZrrX(t!^>I|Wi z1kg9Ekt_mwhqqvmIgl_Vk1sXoVUprSU<{N~`O4VD&Xp}+o{^qK?S#xW*^_z6#pj;j zx-=V-RACZ<3{lqt&<#z;*jK}D(fX~9yX&D7==vB)l+}g-0RBvHdS=p)8%n%)Ej-P! z|0nchi#trau2_~)Ebdz@^CxkPhO*-`e(tpCu2zJ;c(Cy;oEZkdncI~_mT!=cHAxT;t#ud0JHe4csbZK^`M| z@BPF8(Y<)y=C9?vf0^I6^^UcdL@biie}9O|VXPrAOhvjNIcQ!fcH=){p1v%7FQEIH zlzd()I=?19;2;oF4p-k7ZI2g1Ob3F5j`P#cZIv9EVTe@eAGao=jXcj~eG7@cMa$#8 zvu^h#lT(Y3eajldv744b1j5*N%j~N6@qfnmA`GNWZDuJv&;T#YO!}bHzudviINJ^jLSr@h7~sIK7(Ua+lBGTXB`1 z*m>3F*ThqcEB6LP3kkc6Mz7!A78oI6(R?)fbhWu2D8&&j0PHUBi7|3m91}Z9R9zpY zNr!UAHJOK}5ye8dA(gp$7^brRN26wew&_2*z6p{{dmWMWs1-lFI4p=|*I%M)HaIkBix(C-fq4oHsFnwjI-L(91YIgPsmURi!H7@9RFz4Fj=yvPIz?M@BY z2~<#{QL+Lau|d3ejKq#7MF1$AdhAOw+7$Kf?sASrdzN!6%}iTWp0X&~r>7Jj$flq4 zPr+bS0;hI6Cpu?YZ%~?}x^b%fzIcEuV~^g^_xd-H2?s^=bKP zN@-&-ohMny<@4#+td}Q=E_UlDZ?z#__k}PB-YpeIcf|`d+cbVo?wmz%c#S{CzAn@2 z>iuBHF>WKAww3h=z<>r`=g19Z>^Tnwpf}oULM6X2aepFYeFSe3@EoSd8_68O0VVJh ziY(YzM?86tubc<=KEuG*-|nbl8g0<4dFFYw8ZP;EhS!Tq0<;5b;}-bBFa1GfZD*z? zcQJ*`vW7~HCQmA<{OqE0L4LA16H^<%K`~16rhVi&c%Kmx3# zWk9rP)f{_&g3#~w_(lM3^MVN*wXCc2nKDf~pJUItdW3?L5)3=XadI^)ie1w}N!X2+ zVQ3YM%j;B4;@7XKmS%kD91F~kAV7e_;PH{bP4$S?jpO?em!Hw#9tZ+yl&}6cE{H8N z+L`WzBmT|>03XvnQW@~olg?-YD$54W>SDXFS1Eu*b;+nJ+E-6o|_l- z(=|T1dvPkhRyegkMRJ!m@$#gIB3Gx3+J5z^w}$i$#`X2oNawyrY>ayCKAvu)Yw)L6 z6^kizrYb=fsjly5>s%i7^8Qkz5kc*#FqKm%R&*MhQOHrGiDJ`3%V78!a-`dIeML~H zQHJLwqf_Xb0F`ecLPB9>;w6VpH{MwU;Ddz)%)Ci25>km@w`M$$1aQ(4^bvjK2mLFjY|2<6{lqnUN7j6!VVd&TN zp+7XuwH-1}e-E+UlU#>vM2Fb)`|_x@NDjbYfu}iW%gMmf6;_mz9 z4p{L{0>ZqL;epv8IK7edNV}C}U@IilSEh$&8>^ZqI zlVPqCu!q9s)Kl%AVP82xhR==tWbAHIUt=88HhE4w6`KevjNo4%YV&ocuzBGO-&zg% z?tJ2rKVy7R<1pS@klH+422kz#gusaY`R)--<0 z5G(&@rZvw{=O%V^Y^Xa7S1Y#SO5LNu`8|+iQJl;BLJO-pQiwQ--CM zZn=EN(Nb|jej5ONEBQg|{fXis7er%zGwoh`sy^_(gb5(fy}Hi%oC6XTmkJn)V$qVq z`4YU#u?~41$SMUAJ9H(YYqWx=XHg42fWv&R+$URZ;sgv)13t(3d8RmhE}j1B&Q@nX zPF_u@(Qa%bdTCNM%8?l#GF|{i9ielFPBxpOUpQL(=g6hxW+@w|UmmUx;0*`}^+c6? zlymdS2;lDir0x(K_^X5MlkJz3E z08dOUAC3nC>54ZF5F;olz$;tR?tuY))2Y9`^;G;<(A=)|dKL0Ct;gtS3|GgbH;#nI zBB(XU9ov4c*2(z|9g^aJGQ6%IDlvXmX2vwWa2}({`>}>&NiFuVvefW}m6WKD%$s7h zqPUBn-7PSMfWX`y4LV2{?-f;5abQJGnBhg+aRgk}UQ$kcyJJkH5zt$<>s^`fK?7+$ zkm%uf(%8y|Q$cM0OAg0T!Y&I9K_Lqt7v}2R4`0FlTgLXw#{in&xLK+>wK*ZOpx=MA zpYP#8K;JehERp^?>Au-SZ6Nvhk%2E${<;WOG-Y+>rCEhH0ia+$gN)KQNWh~Ixig<3 zs9_(C;=ZyB@f*Tl9QU1uTB;3Jo4>7U_N`Vfh>C=Z9$25Ak@ZFh*ow?`wr;q5_Cud) zPSveS&$lV3@;NzRjV66|B=9p4qRQIMBwY~z0x6ozcBPE<$k_WWtcz^B1m!2rD;5#u zk1E5`jZ)Cr+1Zuz9+uT?gxI1RjzybTdtS;i=sCJ?FUB8%QPsimkHX z76jP|mA|~%f1g%d>>mYZyg%+iBf4*NJ`o?;p}+JiI&Ft0vfS9dx{#J$tabc%l?*FZ z{aMKz^yDaS%i8tw1cynvI5>f~q+n68tJzO8Uk%s#skUs~xW^BTmS8~ZiMh|ziI6)( zHIf)53`uN&qWR|ZE5TG`!UcV34hi0@^b)}{ucvul?fxXg86N7r3BfxUcxv(AGKrR&0zS7yO*qDGBY0uJQi1!JJUm&AXBv*x=sF zZ?qRYCC&Sq9Bjm$elhk54)=8%0vxSp(;9yw-$SiS7laQ^e_=E=TDi%o4^MomUTs$W zP3vIkoQvC5Of?K&4FeAjvd&Y*u;e1Km2Zy0fn#3ADiU~j-<~;) z%2%-z9(Aq!jF#)8_;M0$cpBD6%4;8_>GJgPuY0NM>k9=^hxS){A;s#A?kKSq+!qlQ ziAq3H%=e0`6d!3HR?Gn7%@Z0S*X6fmdlP4p``+^+vj{FV5g^+o`hjGSCm3sC`CZ7T zF8q4#-nCVR_th<{{MJlBBPHGBM5JRm0Ntq*gWJ#_R1KUo*IU!6vY2_`S7YODQS32> zr@KeIa6Fe`ebqy^hB}Nz-H6hs>7*$5pc+ElmlV*taH?`C&{awgCbT00yj9Ko>3Ag2 z=}T-tZ!93tL)0J5a1n}rquxb4UxsxFR=fzQG3#&2o$--A7R^8e6l2iRx`f{RA|oGV zNn=pxEuM8{w$Qk|fKZ{m16VS61cIKol!y>ul0R4=ejvQ*DN;)96_EEBa3&}O4s!b3 zZ<3LWI`2ckZGJ$IuVCZ$Up7>j0N?EZ)37q_6qj11el>u!em@Qn2i#fi^W(s`G>ZPl*QD_rSx04R3E>qg|AN!>5znzJIu6 zyHd>Z*6Ug67yaRiOe^g)fmS)z|YbbvM!)&k?VRv-;X@4Hb&*)VdhQQp#i=+(EaF~6sHbN%9IG#bHWnQ0(6W{!F7 z`^I<{6z_~APqy>`GgQ65x`{j+%eTaNbNL1SxekclnC81P_idD0^?l6kPSuJk`v?sA zt^?ivBO7#JbOGu^z=*xIkh~uihXa#8y-)&A94ggs^&UCkqJQ^qyh^L5fCQy=k`sXS zN}!xTz8e-CB>!ph4=hqVK+Y|JKDSbar~)Tk!-QmBS%4Aq1ZYh|DU2krd7~Dy12Vs` z?h-)i8tFoMwz{FWZ4!}W&R}w16#yL276m@je+!OAoyw!1`u!P30qB5C%ijqQP9n0ic&$LE zjN4%nfX*hPVi|J)I-xx91cxHJ<_}Y~15n;){xZNcUZTA926(8b$WNHiLSW2J9zk0` zlcmTOqI{lz%uHKv@vjC`>?PK47byT9Vt}3Yj6)8|VFL^Z5ChPYGe94-i;Y?F_^+=3 zt#$x9=DEMNi8TGcKUic#s`G$*dx@>T0d)KI14cYP+XmR%7^`U?;{@k)&-P(6`Ml4t zZP@K&yAOa`O>Jq$Z(S+@lTQq~Xd!KLVYgomjGTb+ha8#FcL)0J_Wjb5mWVHrqiAH{ zU)%i!x9r3KV42*6F1WYPIamO3#(hG&b;u;($p|+F^Q}!bBH4-c9xl#ER{(JG@6MM8 zyGS|l{pGb%pp5|&@gLP%B7>mVmvy8Z7D-WRFhPemq`oQ@*%C}-Grd?C1oWcWiM8_8 zuUVst6|l+-J8@avcG}R~C-PWzn_c~j7k&pRj#-FL(FQv2w3K+bwF1G8`fIXTL?Ml4T_@^^^O{9)$iy3v5ly(5v4TuQ8z94UVmJiA`R;_`NgvganuuEBL5 z)!BBw1<3cu1`cDN1nY_g(AYFU(^kjXb-g#6aeQr;evtBc@XSkp;Y8g$%#qty*cn=9bs%$nUkVBwbV+) z!%+PZ#~Z@bg5(O1PHpByfAg>+QE!OL8DN3FH>J=5U>`uzPTgN1h5xL}I?1n_vj>cwUB(|xBI(Ei4aN9FbR#A2@Mq7%{}erUJQ!C*U< z6N5Dxl21tatcf{|dcm(16Gj%3s8Kw>ao)`vF>R?}lpAbz!h(EvLr-eDxKFe-8mj7b z*s~h<+=!ZKyeRzJ+iz)J2{G89XI{E4_W@53z0J12!z%qjgvdYwE%cTMK#%`DSnEnZ zwy_YF5JY*7B@Vrl4WbG-4k4zN@cDS=dc-F~$%!{JUMO1jDSF6oF~W>0?y|{&&AZbt zFtx^ZOtC0Il_gLn4%hP(H|2)o0e!x5=z%%({Hk^jM#l@f{)4VHAP$Fv8`1{ww z;>QH}=mT*f_b>=JAqZO_9@}98vjlcFD4qs@+_tQu&2mCEsy^LA(MYc{8Ce@5PU1V0 zu+pefEXsFWzi{F%b8Y#N7ev5olbJ#>UPBW0YuX1PNchWT(DLC;G7ucA0Iuyy+r;iH ztZK#?9?OJwrN~g@#D%y*I-s_03oFbQ;6F(w&=|&b1QQ8VPaC{&=%0O}R*$O~I3v#H zF1GHbYhWB5Ti-&kKU*BxDt8lHyLD2Ino&_K_0v6wPv_N`;Z=`JV$< zxxK%W5ci)f9sojOTkvbR4WhC%i!pGT{I^S=KlLK$rncedBaCP7&Yr9=t5Qy|>$cQ9 znqCBODU0K*L^Lda{<$2dg}`4A6-t%Kpq;07;$OS#)>*aT-T78lBfh0NqO8aSv4F7gsfEU{Y-sXLIIdDnlD#~Zo{ih~^2MOp#4G_nN!${zFfI=iK`jphN6!HSh3}k14^WoC29j3H@vl*N%$&AT$F8;v4duus{mg=%1Y#S$Tqn)D##1GJ!@Y z;rzB=fc|`eMvo#~Zf{h=fNTOt9geuVwgH78Rk{e?>8eLA{`SHi5VZK;1>H*i336LP z#7F{=#@j1QpI_phBsUgb#$yG7aygOdIcvcfz1=)?ZeXUaB@T_5;{*JSqcx?s&D*W% z3www#GF6Es0meILSiwQAZ4lWL^QH5Uu+(>a6yGDgn>DN%QQql=lfJG`}-{o~!)&)bkG&AO}eW zxFBe`1k?`^{Xb%{f9MB;zfXU)A-KJ!{OgK=t5n6Bn6vFg43^3F^4%=wVH}h%gzMZ-L<0DRxGu#c4CjAhmAQbtyu!6V_qei)QtJW zKmBhiRR8n67VMX<^)uj-vryRnjgFP(sVZrs30CD>xP6wie~RY))b7&m|4hdp%+V!6 zUIa@~e0b-usNp`7N$kGd3G+Vpf}a;mY{+L3GOZ5y@X(q5=Xr?r{AePb{dlHYc5D*{a)m)6&NV3R)RjD z(XiHBq%c=a=3)pKeihnqj`XA1{P*57S9^iFP=JPyQbQ%E&|B8i97>7auQ$ZH_6wdi zL?XPjl2s6Q&^-U-3w!MDbt&M)FHz(Ge?uHGdY=LZ263iEbGU)=Z#MPnQ-HTWgK}Q; zg-(AEI`^)h5)2l03mbmqn&D^)A*x8X_2q+NP!oBW20M;9a3AKO@ zV!!MBBMX9(R8{=gJtJs+$cKWw<=>=D72Cii(Sk`WmN7oQ@B9Ug9w1JalX?+SmUhL(G( zv;wxk8fn;`d0(D-yx1tJHoBPl7(blmQk%XdQ9q#l!xy*_pBrhF9Pk4WJ?&!myokf@ zGV4E_!_m-#raB(n2Z!|l_N!?{P4!$bMJ5W&X)Oehg?bk8=u|x809lCxvXZlMn&$#y zah7*Ro3EdrA!9$SuO2z!&jhZYAt%*y=&Uwn_VgjC+EQK>ZN99kTAZi%+e$p6O5)3i zQq${j)s#4E(A0qh>d(IB&WSX%xCfI0kOt^ErnIVc%c>=>GON8&U(jomF)>TR;X}KT zC7!Pw?`#Hq3#*S9HXeQan7$x#v9Y>ccYIN@-n#G@&)+W%(TJ7Jh_%2>!%U~)nkgMm ziNh3KcC>tjEK)-b0352{-%76q!C`!YKU?QUHNU@<2L0mRmBNVrl|k0%nCF@4NmKiia>KE7lG6T3=dC)?oz`oTqz)fFh`yYWg&RYes76 z>X`m$wAyP$x>rknaaG1C(;n088u(Ry+mR$C*kjTEv#isO43J{1Ig@G>u=53a3&LQ9 zuL?#ZG6S6QX-unScp3!(CV;KG?bT#Du6^d)VCUA>2F%O@@$gG{#D77cM;kzGH{1%QebCz%F^-J?A~RT8q0sW+0y@b~m5F_PkU zHhU*&aGF=?ex#%MY_i(5{%~gAEgtlLSxi(~<=qHS6rygf2jaIrTpVwhZO?F8R%LAp zY8viVU#oj8@-4Z#7<9)biNbu9D_zsH&Gc#sHN)bvwQT<-T|wUx#mzrZWxINDBbe)< zs#^Kjb^OR-X`!C!QA!mmKo`O}uaeFiK}qwd(`-}D!O zusfXHRF}fPyif8ofjNeLV&sFn~PqGh&Bwn`o0tk%7BdW40 zBe`v2lqHiv{^}9?L(ljjdF!prAFQf;Z3pX}E!_D*od7KM^6md7*=7bPN&lz1w#1zB zYR}r5fv>;YZL@jmRcUa`^&YeX1%C$aN1fW-dufJa!}*Nqr=#?B zeVu<*J;pcbCU~Ung{M~8LsQ#Xk(nO)h)#B#hUNCw(97^Lqf-MbK38&##^p4X_N6D$ zTwL?g8@_=9dio<}TybSCB^TqMT!NA2`Y#pjE9Gk zp9nr}{*~eN^02=Ghr^3Z$&~_EA~3`)jEMy@ zz8+p)TS04#uXhivQEBJNj?>aKYsIdhX`S*}s<_BXZyd3d`_l5zVeF*a8or6`T;L_c zjZ?^KAj`zlfw>gRnhd8b{3r2rk*>R<0p;4TqqlCO!%5klv-3Mqa-GjAzVNGXK4;P) zjaOpuja9QcW+~x^QWl(`e^W7aBQb$`YkqS=ELHK@-Q+L*uF|;+aX+CHW;@fmJ_YKD z(&ZZS&2DB0=hptIK=fYU@#ZCwe1RmGTcg^-Cvo!q7AjE7nXB7mLD-+6DtK-VU)ajh z(mnZ*(iHSq2xHXrD}#)8J3tOR5dK|M!^S?=TxA&~^K?Opef^+~O`0iNI_6&Oa2BQu znfeWg2P}|rqmyB0bCj#|ki>SOGlzggoQX5rf#6tC(7pZjv8Xw~YuL5DIq{8%U5cEr zUG8ePlq3}fzghqJ(aKyg_ww4Y4YlE#_ueZfPnV%s5;C&aGz*`K1qy|&XzLhm4wu(@ zjHj~&BS_YoLCNy$o#kzxfy5QMe-Wzr^`PEl&+sz%kk4zIZ8!%iH`N*Zh~n;AiNf}x zWgzMGaJGS$<@L#}<)2dt6>H#vQW{@wFXR-g5@$&m zu9;eolJDwt^o1fgztOb2oh@f%Lyy;3g(#oBMSBP&44c<)wVMf;Paa_vg*On&ZJnfD z5YHaHf|MU#c7zA&O}pR83$IdTAiq>l8Y`eqlPNTk44}lHTyeH%EsM4Nokj9eAd}>^ ztKrJ3?zD_Rla)Nq>Y)*=-^;7GPd{Qla6mXW-?Qd=#RXfpfo&Qg&*rRpWT~zrNHAP+ zaJb}T5Ys_9Sjh9qq!%z(5XE$^H0syc@Whpw+5+X&(cKEMF#y0Xr)Hb04Ywf>ZOd3= z|2_ZV01^;4s^)3pnsWE;u9tlc=nGRY`~3Yzu@rM@yWO?9!OJL)E4RSc5&YmeMz$aR zGpC7*CG78rNWr23tVy-+DGVp9UTuG(AN($3;7BTn<(vXh(%)f6d81(Y^ zxzs){S1$PA#rY%akjjC!rCYjucKxW&jk=K_o4sWE+?e*HB?bi`#TLOyOz2pjXFDjZ z($}xPDSakbIY>uSI5Ih9;K>MUF!~jA-7yb>XJD)f+xY64p4uR-sA z1!>*$6^&r1z>qQYslo0A(MN7M{eTZKNA8DXlr(I*)J zMPwT9gMrJa6#MaDBto(Z?8q@Q8at8Gy~hD`Ij|#^F0@J#S&aMmcx_bEq=(@heeK6j z0!q3{@18fSA2?k!FUJxv29*R1_+c0Lkjt^N7m(P@iJeMe!L&`TI{VLg!E|01B`{4I_f?{*+|_`>450xXR{EaWu_ zINjpZ-laTU&v%Q@V;L6+ABEB!`G$r@Qb~}Xc2yffndcHtUiP;5qV%6Vd0d|L&o^Yq z0>{rG%j(4vUq)JOQPJG*J@V|E=Jp*xxnHKrs#l!UY~sd}#w8ouoH*p~#Hg=;?K%eH;1GF1q~PNIs%QuQVc^Uq2aJ>|xGy}hNI zG>t?DQwa9cuLG!`R@EG6jk+QmjDSgyD*L8{`Ui6H?xw0ehQGveI~ZnT-*fG35ZW0- zRF;?&w+!{th{dD}Jok#Hpc#-d>ii5Y8q>bNSv2XuW0adj43_cD-ZX80wbqi@=%R*0 z-~dRewXUcQp|2yCjJ!ql&H%39DCbbUzxh(bVrVIP()4OHTau7Ug(&@@#~zhLy?>p$-5qA^S`UZp-?Qy6!jz?BA$G3n8@ znDFq8Hs7Bm5iSqyyRO}ZZL7Zhuq+Y-YHp;fH!+p%J%OS}OcBFzD;$Szfa+IW9)-R9 zr52U3WWFsjmYrsXx9f+YMr&6h0&lFhz7{Uxit(7*Q^@0~toE2*nv|v96m{Y6LZUDB zmdq;MiJXuBu13^y|2~EkV(P4o992wfH>O?}F%Swt zx(1B$-kt|&Gd$?E%Uzh}#jd~%Z-n_ac=v{UpAF~9({g>#l+-vp2(@fns{#&aC$ITD zj~SZcnpoPhs$4L`9)R#I9?y2!CuA4sm`6u>yfg}m{@ICV$qE?tthJ`+5J}RggV75k zS=)ZGnK$&v-`x=B{{5Fe&&`qS{8a)Q5MphG!ei(P~{-9D4SDC>r+>6kE z`xaNLyqIC&_7`;affyTRd$*w9CBimh{d6alhvU+7o}7C{Lcr*}9mGu7IpZv1VaTCT zuh+4f0G=Ajl(%f?)g{=Rm&R^hks9(G{mD@;EbSuUhhAI!6NT)|;0^#pj@4Wq*guMd zeFHc*C-fAp-CrxGhiiw4lL}%*I-O!FK!a zb|(n37kKo0?npYI(@NL{Wk9b{6!?7-6`;(cQ{`Y?x&BSB>rK6^o-#q^s=u1R3cZpp z&+03#e2;sSMBvOY&Asj-;AbIB49f+|vjVwczM~=(;K$O5yZjDPtj3y;EnOQtFNryW zp=R3`7Tu~Cf&i$de5nbzS5!|t*}`*N7m*U)vw{CX*INff)qZQkf*`3wH$zHy$IvMy z9g+eP(mhBD(k&?6-HmjYG=ek&LxTtd5|ZB@pWk!NdC&WP{|(H{-s@g_-7Btj-D|km zcoI4lG9%xXB-l@RUcz}BX|>Y|B4!5z)+P){MN(hcnPOyPbaW)}L{z^cvi#<^!M6V$ zl5#?R_;sM+gv;(woR9M`u8+8Sb1+z;j96LsQKCs~3B!cpQs~@0a1h8t?*HC#Pd21F z_Z996}9PW`~3;J9f?5=$FNliEO2JlfTUB(k@pSUu}|5c^Np@3BbWR zV(4|J+Nnkc@w6_|z`PfER15>*QTG|?nGmH;7aFrxB;#-LGx>)RdG zm@Thi@*8MP6&J8*F>e7x9*1ZSIlE(>ToX7;zc1;{HPLCzLL=2=z#lS(10)p3a_;}V zy%D387=f;#5I(D4>ugUY8Jg_M_9VvgsN$#4=!}BMH_Tg31zF!0G9Yu%olqAYgZ<=A~l}R zg|)Ohxcirj(p(5QR6~LC-4*TaTjALc7H)A*71S(%I%3Cv%wojn9rWtUE)we5 zPD9B!?^PiN0G_$75>be{$Y+i&i-Hfd%L6kOc?{egl_mO5%t|RFfCe3)#3I09_=5j~ zyT0xHU06O}d#p}9kpWutDSZ1!lgWfWdlkSBf69+z{q^g1KsNHl(c}k!rJ7X*R`v$# zf=GKCULSItn;P=@t~t8Vk$}hi@cyE@k5Fj=$aLN9FSh+m3{*7yDo`hkYdj`{b(3qgb~fON#WwoIX~sJeT#%GAf||d_nlNR7 z`AXf^fBVOoKHx3aSI08<5Mdnkj-c^npi8;a-~P9=5C^nxPr|KsQ~|VB9`)z;(6#R@n*4n<4H zKLe5vL>COWJY8C%WuO2%PZ%jJ_#KSM&CYX0_n z=qPQ;ci#2_A~bY1@CtD&XAj_GdQMVv*#PKU_8-vKd3#Bm`3(!5~LGl8DV&8hqcvHTL->7-6(D0I^TWSPE zjyMG@)Q+K#P(~^M#o4csVS}0kcL))7vY>=$xm2D&O@Md({%!t9vR!t_46Ty(1#kZJ zeuK~`H_+AyFQt|M8avH}fnvRVm7dqusw zvM-ptiP#CK6`bjid^o-zsMuqJCT7Vhun;nx0dkC;ncE^Rt&6a#XEVA!DNtI(-=2uU zb@+Uo2<(u${htOr3qscBG+@(|H-0}`R`K=RDHM%M$ii2FGcMB?hOcVrL%VZG@t>5gRmk# zzDw_#Sh`7o1g73?fC8a~0K>1a)spFwRlB6zKRz3dEt;n>s4 z4pbGN8T*0_jMxB;3n6wk?m;^PpyCGP3vAFAfXY*@SDnBk$3q`}?4X4Y ze*Pj0I5<#~3Ou868*hMA*iDGocg@=61Imr9hssTDKayGRUm_g>*Odhr3*8ETT@^5n zFW7?$?U^CZXyQf>8wU}<^pEf*jPI-p^NlE1W) z8BD-L*&7o5c{0PRv|2)E1j8|g~U1+*_1hgc=^FmJZ-VJ;c@NnJ*_+uk-zB9#G zjz5}1Z!Ul3cy=(fZoJ<9*m8AW{%d4~53x#oF$d%Iaf zMuyqiucN)WT-;M4rXcz8<<7bL}ZvMC(&S*+mBV(XSY3^W~5BNmD!A>0kp`20C`* zks((W{btxyf>?{LTXR)FeKymDj3BuG58g|+0}5aOC=RDTpEvtmikE%n41vj=E{;uS z_Vw)q+)Mva;>Qv7v`^w#FD3^g4yk1bd7}GWgJdr!#(O`6b?2i^H@$d@O(~43^A<&R z5iCENDdr~~66U}5RBn^#=zMAZ?Z)fnT72|&X9Hu-fX^yS=@NW7d*9!80p|Sg|an`lm=LVJxXx z0)rbHr$?fD;;zU|RHC>=oo~)|drH8W3EVnL=@3&0Ghn~M2yZnr*RBO@lE6TqveUNR zZe)1O|)w#42@k4t`qpqVp5tDNuT^+J{E_LYnFf4E~HDubUz0RznD8cVI*y z5ZY`5;{!%1!h^hBY4c6V&W{Ts@CTY>NWg&!eNW5=U@iI$R*ZOzFUV}6`mN3kh+~d* zR+Aw_4&u^feEZI{CtOx46fw{r@LIETPkPlfZ28fr>0hkPED6esf$kV5`m^7;UzXnC$_{e_Pw#x$z}_=9lx`Q9vKw}xgMPsMuui7 z{E~_VI8LwJe`%t+qTTF{fLmn~_Fv=yXa29ZxEma`pFZ{nm+NYNq^rvcv!2X?P50d> zrm{SR4aN|^J1|}rfX%3>PB&?JxgXz1kgyBFc8|H-Mi$WBz|W`_f@9eab2b?};4dbt zXFZ-}h>*7*?tN7}W@RZ^N&QAQ-6e)$tZ*NJCFm5wOeUyn-yd~EcwMi3{9NkhmCn;P z;?cy%&ri28*}E7|2re>(EUwNruugq{^X_hJoZv!3M3)bs`T3-^dZ53r3AQ*(S<3TCFYa|*V3KB z^9==TeZkM)J`vaYPs;|VlMrMeffI}y!hJ$@QXQ}R2WSc8g!hg8k5(NbX zQ7gBEF1J5*T5FeD>?Kw2bXCPB7aMmLt{gJo`q09pb>HQ5`+REH%AK&hGmeIiq3oJM z$XfuqJB@D@5(W?;*5UQ>Q#IRLwh-?>v|}A|d)|s^C`WH~xW_ow>_d;e7kq9THYUS3 zhtD#R@1iBdeNAWsOsZB}(Ecn-cHd!*&{h;(t-{Q644;7+F9^WWJ%Qt{z=AoE28` z2UzhfKq5m>Mu1LIFMwqbsJ9E5?5&kNIW2e@3<`VMXNYzGWo&L^<4?V{e%m`2o8V{n z_ikIPdvWg`98VaKy1SsMisX}gV|Gi>2CTdD+QYMtd! z!=Hfi4OJj^hUl?n1tZ=0erYs+*)g2TYrP}r2Hgqxl@j^=?dG2u zoC*_h5VKa{;8LYRJXuAs#;pYs1KC)0ds87$j6k zb<)_WZASXE!^qrC%7^+#^ig5@19llPhguLmc0Fu%mZWGMcs!F7^6l+8RzO|8p?t*A zxS#zOr;&QgQrK(1HGu`UeTL6qNF|S7?8QF!Vp}H;p;tDLmy@mf4mClcWTzzOWw%cU zV;6|OT&w0m9A0HYb8e1TF?#ca6P=Gvz1#f>BO5$P)l1(}Ub!E}Z8b3y#7#AUS8J?? ztgblHLeMT9`vw_EM_P5|zI67-zI&#HtBlz5gK^~6)#)8t6$E>**N+kYYlb*! zB=hN0qe$v7T{+6)cRzUbdcQM+R+I^4 z54Hqsge^kW^OX0szI-H^nGKX_zWMpQo%NHn_ziZAQU=cCj}JZBzOB`(p)oq<4bJQs zyX}h!BZFke;sHGyhhDPwin50q-})ogHCk&*l-c&Mj)tn&TyAWcnzA}r%n1v4I=zhv z#y(?|$7KyIZ6+8qmY5{g4ScBn^C^r57%y|d#11+z2PX}A9ZhTTRjVV*xmwCA9Tr?& zou}((kAkuW590!Yne*{M!ZXD^UF@r`KQYk^-W&~-E`u6-O=tofr&^0|D)AYKOshdg zZ6WN|Rb^vm5>Lze258WVoFv%h+*s#7I*+7jteov^T2p{p!y1P;!o8$fJd_C}hYsEh zK-gD&kl9&3OYcJ#`ytcUUDr$~tk1Fs>c6~xbS)dIZCtF*v;I zqsFzvyzL0nZTzCbOdJU{8smd;!HQumg; z^Zu$dB_t#>omQ)ip!ef+=PS|`Om7;LA(RKdtCQ{Y&-I0`Dh(?!OAPp^1TtHrSy0bl zTa{?i-IiaJV-Dm_Bkp4s8x-tY*8=V%dB0H6hInYYfMNp9_E}j>Eo`5vI2>W%S@Rt? zye{UpRLVqtmin{5uh?FcX-D6i9fIu_>=-3jM%kC~EGC}a_^k?-5WT>#t2wv;?jYoX z@f}l?de4@rN*w#W>c@WPuj!9lcnSzNkD6@qdynYLE@l=P`jY0r(iD>mE+{uuCH@`J zZ;{{4SJE%|-~7QB%z_Zxh3ij2B%pAzJ3V`iF*k`zmXf_B!|kji;5>WoW=(jEH!hd`<{o-| zQ5`=2rboXHnsb@{!)@CK%PcPx5V$SU1z=<3wx z@9UE_3bk<)M);dBqtMvDeq3`R4WpnH#r2EO<%k(2l4G6M6au?b;R_r`X+oyt->Pyt z;i9>)(h~&hIcVAMFfq9pToUEzV=SROHEPkZ6iFEA*Znn|E&Q11r`Y?zv-yGQb7rvO zqp{-!+256yb>q#&TUnB(AB&sd1ju=Zc~4pH$bK3;eM9Lc?`oKLVdZ5*FYv)a@TkYO zuzLP?NOq_F%npE$v5B3`tD0xMw4li|3H9s$X{<0{#a&eB7UNo?wemtAH2CLeAXPM= zb5LXdRe3v(FoBFS-wC+LL872pm%dGG5ZXfXb5swidZu&R=~;Wbr&-@#L!H2iHlD1# z5-FOi72_(sp3wNg6PfRaPRFbFE7c+sreQeGu?SZcHVa)672k@l>e<(Ahof!7*?$Bm z)ryxK(MRB!znI_YJ>Y%Y$C1%?qwlU`eDn1q{a}Iaxm>_!DiON=&jKMRj2JKL_^|bB z39BW1@xCN@SXm~%l4kpsR@kU4SLZuDIdW)lz*(7&&Fr94_2RBjWaKz?w(C7uM?>6{ zBct{Fk;UD`+nFP#!y2d=948-@pg!FI(jQH=Hja`Kh=g7Pox(f)-SqUsC#ltW!KIVL zWx2gJP^6@c$7i&CEU=jC@4;1;bp7j9qwK-qGy%d3uw(%5>d)Qyn-j>6Zz6`)1vw&MYZi*j1QxuB2!I zIy0Cdi$&;KpT~K9t>U7b?!7ddNPW;U`|^$bHiescyfr_(O`u*6YnqlYZJs??1YK%g z`m00awSsQYJe7D<1>ZPVIY76bvc~O_fG+%siJy*`gM%6ce zgp*7Umvq#ww62OK7=;1QzRnWPOqn<~h zy~<_`(q0R|>#~fY738gpW+6_~Qw?DUrJ{Kd+9i&{5qYjUELqeK=`uI+N$Xyh>=i!0I%r!AC6O7S|Y~F09?-JD1{w{>Lo=2|khHkqs?BoKHWZ zm+5ez)?4AV9DV&1-f5%2wsoi~oih`34m)hoj@%sy3Z0N!PZ92LR?yT-0=edkl>0OW zHR_mOjTsi7j23fV{NBz&KDIq$H!yRrg~&#a$6wROm@ek!tcYWf#eECuPy5vg&CdfZ z)g;eR`*BnXI=twq?YMne`Y5uPS}Nl84E8kgo=&|I?fw1d^B0i~n(VY!Cf`uN^M zb;gQf#Djh`c;zPPFqzeuU|Z=yjNoXcP%@*>B5Lh zqU6Y`nVX|J!c_n33Uy%T1)yv}|JMboOQ!G|Dtf#mW&5Zw3Qt=}7s9P#HL10&R#^_n z#~ZXaVtFZ84g-(

Y_r1+1|*llFXHa)5h&Job@GK9b0VC7)OBCW?61bnSnCY}Gm#Uc%%-q&x^7GBvk zishvr`(?Bjiwi0&XZ7oCN9A(b`-74HgjGCKzsY#Bnih=Q`Ds2;moifQ#n;n7=|5)& z-%#ontkr8F_G?}Q^G^2|IbZ6OqD6FAshz6qMH(v)Rnz)|tB&5b#tj5BNc(1Da3L|| zNFZwt7IHjYooduDzT>Z$#fw6ttkn#3+n`9*nBzuM+L>VRU12t6eS?u_jtzT{(o=Df zpw&{Q{9Po^g?;0*Y&ukFk`%6j`c>?OAzZRpP0OlqkEcP=W0?3Jb|;}wLEZ+<$#KE- zXAmqbF)F9;!PS7M%Tq_Ps>gQdfBTNLuSHK>rDBN`ADP?#SS+qWT#rhe(B(;zLnM!M z!gJYlY1%X%IgNx0l(b)ob8CZ%$?N1jc(Rw8E1-dgc)~%yTj^S&=in{3wMnn2B?ZOL z6;kC*8`l=!^$Aq{G&{kD+@QFiY}oC#C`)K>m;@V`eHlovhpavW;6I)%yH8T zGDP&$6!am(xdR@lkMPZqx#_xLxnHTMR;0w1_GM2sPG(LT)4c}j>zXholN1Ek-Z+3& zIN0*jW{G;zo-(lGn!bKY17i`+B zZNzIfqL}_>Y2lrk5!;SYi~8I7Phy4<`my1#_x`)f`Gt!7 zO1r-Cq6BYh^BTI0iA#%|`IDeIGF{hf%WMJ%n(BfB2H1x6#;nBj-xof`L=p^+sL+pw zCVkYy18t=#aJ<7+uFXi5DyC!|%2k9Y&NgV{pGof8Up=+qY*ujRp4p)k_yCR1lHyQ7 zi@l2wv)wTmHLJy4ibTV!cmJih-@YjKnWGdEK+M4QaqC{5xuB69k&+FZr~FkCV>hfv zvH!8(o1*GddQ93@{As?K&=3KRhWEKwq^~$z}8uG zU>c2a&3!yH$X8D1FJ~ILA62VOfHZUS()@7IE^_TlDUnd>>2>&Vo+7!u(xP&yPw5o#Y51b;lAZ@{*v|N}n-Rsch3=LbA(ow8PnL?-GP^gyO={ zP7;pA983xypOr{$Ygf`Y6u9XYguSr!_&Oe2$1X?U;d)Oo^m$vV1@tknrcpJNqcVMX zKWoCH5f{`C&kp7Pa6^kRdNXOdaTrIXn8k;2`bLdYbMIUGol_r20z^8!7bZonWEA#y z<53~Afum(1Q^~Nx)wHOmUr!oEX&tIYw?`wSz~4WVqeY;()^mM;QtL5esuWdUJzO6T z)usn*((hpjXBJ92qAi7^l^rHM4_1-~zZy)+`m9;@E%k|e=Cg~um}>2U;O(Tds~P?! z%dx`+5)6F$?vxlG=bec)t;qdDNG78sBd|B_s2xHj z0ERWCtuZm45`-5tbkUdb6xN0Kr6t+O7(~)GV+@2qbGX&9hZlAn(Lo?$TTNz30~Uhx zggF7yi67o2&b;|{Ng>g73DY9^^~!rVax9iEBuh1+LPxKJqY~-CdwzM<3nNlL>BA#H zE$z(K2OptRVoBiSiHdkN#$sX8LmfSP$9*$#ksMBj0UX%-YeBEYcOU$&&?iN7p5i^^ zynsyk2u~JA)L4GYjn*vUdh^+xiGJX>`hJ~rhqK#^Hc(H}_Rs&_#@E0$Mp&q-ZO2Dc zDQn{RPw&FC2G~sEnqTP{&ZRdC~46i3i*~2~*h*!2!CaBdDjCa7CnDcOme2a9x)W<)8vA zazZy}=2KYJo!k9;*9ca)q+fgmqx+uWbuLf2v5ZSocxydN0l%D^H}6#odc$Pm zv|9M1cY1ptY;E(Izdu(d%w$r{a1`^S@jX=Do?z+cGU(dt_x#F21JAj_CTC?WP)<;( z$v>o5Q*>9qakt4nHK1(06&6T>##ew)Pq*J2He$BA>&%Iz^*_#}n`yA;b~kK)k@18| zCzqGMs0oe}hAuy5QuGT<1ga~FVC6+(=teP@4IBu@jX+C#feEeDpr@kvc@+Omq;#11 zW%>a?>9H7HESJ_+98ib7uK>l25XgO!_6{K*n?IJ2m8@FZAEibA{7m@f&y64xe}(pJ z1X`=S>BM0&#|JiB&5g6mV<^G3yd_c|Fw?WJBin~CX*iQ`E#S5v`ry}mu~)kAC{Z$p z^P_gyGnV{7!_uvBOU9LCCN2EJH+R=}jNs5tBGWvONgE~eW)gEa8d4ES!#G*<>q>4zO@`dS^=n>?KxWg5TSWF13+Jbz?Z{lY5D8oE;wKb%6tth42|x`+ z4Bqo-Z!~G`hzuI}06Eo&(BDbaEf?bIhE3Iggh+o&Hn%XxnZ63xBJy9I+UU zx5I9^)nG$uk@UOybD1L;frdI4cfL%gi8&K-Z0q1~@@E-DLq8lzsI?)v$pr+0M+ZPy zqVZdrwb|Up&419hajR#5OB&PV0mqDM_+m$Fb6^SDRE)x;=xD3DQ2@_9MUF%S zl4k(_7N}pr!LKnx)`*zjiYum3#AYO4V1? zMkAk3o5Ao@b$b5S#n`~Vn^Kx-7|hsE^dYyI7#4USBpDI&i;4UNw?9RLg|r5`nXVTa}%VF-?7@Bd8q;*iP zv*e_(At1H>if0A~cC#gMb*JbrgVvhx)Of?;r<Ox{b)WA!F>QuuxN1S`})5$U=rREwWWhkhizjd;cmU;!zYD5lDoc!mZs_ zW7mqdv3%v5=LFe@&IjsCfhwvfU_~U=(0I`!7dG&iOQLGtFvZYwFIyKsz?j|tSx2mL zn3DyviliQLD0a#ZuPp*;(+;Wcm=lWG95GeKa|hz8pV8{*huJMlr?kzP&6bP=7k5NB zR7XEl16v}85eNhNkUd*w7hXU)Hp<=cFsWtn{roHjcut&kiCyqOFk2lP*m46l0=zH# z_m+>l2!YomDS+&E$Uv-eq%Le=S{!F{c()TO@W&Gdvr<^Dg2O!1i!eG0 z(|}vqlX5R1mqYPh@qq8|bOBcx6UkBsjSq=J>`t5n$>Gs?bPPXKpSFgYxaVFA(2b1^ z`Six1fyZ40h#RH_(hT%PC0E0bmc0s;)5%^W>JJ>K^S7{Tl<7K*Bp*v~Sq#0V=qeRv z#cvz}LS0#;$bU)Rj|!Nbi&SPdqEMQ~<0^o16=_!r=tkscCelaX(`5U-L%^M+TE&vb zZ_?rG19mZI!w5s$#{B=gMvXQ+=jg+fH*1o}pP!iuDRoCVgUi68Wcy)KM6^|7C@GRg zfO+A}^L9@HMMQTAI*I0ga^oLTpH3&33}%^ig>767Kc1ba@sIR8ck+ZzzAA(Z$V7x? zO9YtTk71k`^Wl4}kXvkhSR}idYiVSh$P^F#(`l%l=Wd|!u>7T($xui+=`ySN6h&9} zMMIUs)4N;v1 zVi5;MbM;jQ`)C<=GCO2_{4m+vqUr_2{?aroA-FB-o;50LzFJx%KLh`5t9PC(aP)Fe zZ+->~1CKti>Ad7U?&CJ|OHT$`pe74IC(4&!@(R*OKI$$`L z=}t5i9KG6j2Ux*g1i}j1YCtJnzQKFmFSChi$@nT?!l#-ddCQ301LxgMV`hPTSfrsO zOf~}V_1M;*60KVFx3lnYT}Zpn4cXJTEjeZzctZY`*^yA_?{zTQtm|O73$Mm}y+P9E zOw*6mWx=V2ELIUus=#N)H>^vB5GTx?$&^5kv2h}c<~Jn|(W)q)1}^CRJV#HPhKEW< z3-+h-HaTH&heP%(QPMN(`>0igYPw3}cXB?toLRX|b~Nq>+&D*Vzh}~+P>O53Rcnsk zh(kf0Ym+8P@v(zxB!i=Gwi&z#XB@cyk5_Zo8kEaGh@Y(VgYs0$*YNDO%c^ zTHI77w2NT$aM{kec7%srW{c;W9|xS!|NgY({nb2~D6BX}4wD?%lY@jn4B=A!>$OsL zMz&3Ok?GEJ6{s(Vg{ZDs(O1g*8LMjiM)xDx4&Yhp-G?ImBZkBQco0 z1AKQ4l&eE$RLlyQL5-TD|1nLQ-!ZP|Pf->&9PMw#s=zsE+xT7q;fe}8xjIG2Gk=Dt z-(+2slI_r8jpmf4n>^9iX^vO-hs0SOesPIXpc9^-`E!#%h>g7+7qesim3zisaus{K zS#`ECV~rw)IoGPX8x*@SK;b&11uWG>{q;znYD^So6d((BYC7MFXA%hnpzaz^hZ%qmET@O zlgqfoL;j5JnPk>40O}8Kq5OlJD$RQSiW?}~HXD*}`6dr3Osvmi|=@aC_EtC1z z1d^(yhLgsa+TrM>nLrI(%ExSg*WjlHg$lvs=)esQ3XWo5PpL0E_YN*EPSKr?O|(8k zn0=PnH#S9_!udim2i$U;Yc*3ku3m<+ zc|14o=;;s*ucyDr#*EbNaNUP6ZNnllW{9Kgh=YHHX1983S;M)!hJb2-xMRDA*+*C9 zUNw7k zxzP22enbvqh8%!y6X_k!t~Q8sc5<+dHOofjM=gRXo819N?()CG`y9k)d!!vdCFS%c z%2Em-jd6K5RnMJ&=FE7%s5N9Zw85&FO_P{69>1>OT>VN~pL;kBZW;l^bgd|Fbnic8 zXcHz@8#soXRA1~8P!>*sG>n#59FzuF&K-V@C0vsH;ZToLHi*o9lZ+vbGRXfSIyhg8 zhJac3Rlg|8RrSCiC1X6l566Jm^6lJuz{79Z?4JCM?bR2D@8YPwwcv8tESCz3TvAln zI1Lsg=y%wIZ!fcSP+mW#g~(cuK7M29dc=>?!)aP>ESWk}*psloDYX3Y-jvfZ0fXcz zkW>I=v=z6j0>UjhAn&k%#RINCbchPFGFaaZ-#fEtW1ruPuE(G}DwNUDn;n2a+5^xk z$>$~U?Ckh1-iGn*|8zdPCcJXee{V1B+UL~=-uO}AAolK8|8#JT?GX>(>Dwq@)JW=+ z13CDyRiy|fcb>L6J-CEYWH)y?Iz))HcYvuwMm^ruhotns+g2l^`9~aQ1J`7PIC98VOkfokvWEToiN((iwkr#ViY_4E zi{RRhCzjX$aseg@E+_Rd;%`h9_gEK}3q$-``D(w7LpR*>ViJhNcY72WL)!)rwF52} zFWk)awxw?gm#be->*!r?BXF1SNWfp1kpFdoSk!=M$PA0vp~J8qY=(p|vkPFYDR%^C zkN!dlR$je)FH2K_aBWtEKibE`3dFy$w<6wLf4_Z>*cGgO1n7IT;E5+cq}!ozb->$d zkSUUsPixoNG+XYv{-RD>-)c7?} z*IXbaRE|b?w{8@bctBqK3_h%%bsXeWL?EJObs$dRO>I+tj7l5`HZnKL29(TT z7KD&|y}-aM*<2DTCe5e}*@pyY))OT|n*1=zo2BUHifO!nTz*3!%5FeoS1U?XM+_}=~ zk7zQ16dVU!|GCP*|MM!qpz4_TDZQF6%2+q;*n{%fPJ?j|T6DcM*pct6>V@*gw=!@G9rff`6yIS(3b)Cv92 z;^e3CI7pVd@G%LQBL>HpH@=S&r||y?5OO<32O_Tw#_lNz->s?TaFW6F{U2V&lP(%D zls55Tg}%|Y#O90j9}av#UL_YPY5_lZBs@d8>tO(-PvK{B!;}bXfB?^!E6#gMLzcm8 z47PDN&uO#VAmE5*tnjg5PMYub`VMQp+DH;e5kg}!pItr~ho23B-0jxT8&VtVl#s)2 zQQ>ztUMm=_v8G-A5hB`p29T-A$|WzEIDb0Yh%h3DR_s_*^n3v|l1->J@b2dFgkJWJN^AF}EdQ@CCiL}ZD1 zU?YNI4;^E^l$Mjr-R@*|i8@;wjx@cbP2a=hUa7QXgM+T2GlzN!NySI{fslH}`1#sE z9PsR}H}4=8lFX*}!>`InXik&pmkWZGIS0|m74NguvGO5AE%KF3_ZJheVFpzJ?8{{m zbgAZrJje$2C7#4H?W*4$j%gVc?Bnp0=T=|J@Uq{J{Y}dIy6*!2#PACRz5+;^;ie*` z4VYhDZ%KS!Wh@@Y;Xn=>-F=zT0eze?s-vk7cHJ6@D(;Ef=(?=1(OW#_a@pZQof;)p z%lA8LHOKGp?E@1|X*m35j6R4{wM))d9WvQlx}n1Uxk!X|fs$h%QkKHy$Nllua=52J zg$eDoYOl@i2CH<~O zt*E6hhD)4wDXFjtTqSqNgTmge06k)GNi*)i{9*JJc6=wyzo8~;}OdNO|h4z?J*q;K> z-;NTXzm|y#@0ihtlNa`4u9&~7I8I8g<_Q^EHOT6rHoH1#dK_5;0NY%BW?btH31^lB zVY>g)!gYiD(105w_s7n4A3mK@1z51GV#gB`5^>K^X}}2c!!GbRR4qSE6+=lx+c|Fq zD%a_ECl_dQ@iGU%H(S$N)GeV=5+1i>m;t*F50D@&w*C!^^8! z89U>z@tXamj6A}!?Fvxb7q5nKLEaP@W)5dV+o4J)n_)SU>kD|G=?LU)=JF_TNJb5 zIBEm+C^}q{6^m{(GV(rh&cA>%TfOTw)~D3umCsCyDM2Bh z*g;88ZB7?x^uM^`GN_maNp&nmP+PXZq+qVcYB*^vZISDn<31DUc8~qud$A{I^%K}H z`Vgho@2Kya9y0LDnVmM5Vrg_Vh0Ouz11=`p@mdGJ8l`Bs$zz50$ejQ?k$56Yc8!Dm z1ip#3ut1MrWl9j3xxj(}{H{s`iA14chu@jVsffsi)jA_BQhDFT_73A4&n~9-uaT24qa%rV!ppylcd4h;W)#WevkQv=-^us5{xlCE;Fp! z9Pq#-ycKcI38`PkUe-!=x!m!zHe3*-Th{TR*PfV`>F%!q#B%A5vgh&a3XUkKCI~`o z=L`6=EY(*L(JIgYQ$aReP`J>o@WpOH67+>~nx-vYAvxQx!OcyQDik(>6YHfmDnol*O5j%;dEbJw31xSd+=(A>#QgGn7 z9nQsl$FQOtX?Z*OJdUrxk;rr81pr_7G>p8Cdm4qt`yeyc2Vlqm#)?4a2LQN%Z{M^y@dt^yMsZRjFUX4Szh` zSw0-Eb#17r=O`*HHGBeHKP6=aQ))n_j&~++i2-1eXx_ryJLl$WOw>wJr~jYmPw5vu z)1@`x%Q-2JF;DLk%f4e&OJ{Fx1h>&Hc>hca0e;mh3))E!&NIK*9%cN!e>>Ew`K<-= z&AMTzKAw@5ouvi6a>F{D=ZV5_q5{mBcjz``s>#hMiP=ZNMM8AdAc-Z{E2yZWFX+D3tcHij$bxW zs=iUNcB-7|c>6Z0dPoduKVGPgFJ4*4p4gQ&znQ>8>Syhpy@gZuKyd9O&LzOl1xg4j zL4aRoVjnwN6v$@e=v|>~w;p2Fs@zP45?XKtQONA&}0Q?%!f>c{zgE6#_Q@tT( zAbLhrD+`S$Lm&1Fb4PlM2a`&E4;T9@CWF}u%GC$k&8+O^y-W`BY#O)ZsH)h*Zz3G6 zW0RK1@^X^NUW&gS0eA0;4Mjvn%oqi<4(%vFF`_z<#mwOdb2Dg%P z-KHLnar2whL0(mAD&H*LnR;4M?s}2i(29+Gnz%_|?%g4=U_XjVNT9&6y2?;5jHU?X zkN{SbjSa4+*60-R|TPT{ttZ#}0Bouho$ z|AK04KNr9lYz4g8{#}4d3b3t%TXTi^_nG`Dfy^g~&S}R4*UJ&re4xm0*fb|R??*GP zU#<5Y=5Hzyupw9Fc;3qN7fZQnZ8Yz9=d^R2&$$fi$j>VchHV9P4=otRdrV;_P zb{82oQw{uNf=S zRIk-d4;G>4J)dJ(;m6U1C7StFC1?9H3fgWi{nJsP1g_W8vt{Zy^~v_KRgl=;k6T|H zg{@S`LO8P}UN=5S32m{K*xbNc5>te234X11gaBp9RK+I{nRg@Md}(HK?j+rLzN?2| zbSi`@i$4?2{Zp)ovKbw~;=To!y0zGlwDWTBrZO{i-4GkoHn+OV(;o{qL{dJ06j5+@|7F)2zpLQFjJB z#EMGIO97=xPfhKN+|5U5j<|Zd^dsNfK&)N0NKYVXes+AZN_T#<8OAlsv`a5=Ao|Qy z13g-K_vPzvDKz@^+c-Zuy+cF@qt+J+6{LmSoz_TzBrLG8@u|4~Z4NE?M~7d`^>-!M z@)sv2(^8Qt)0V&~;z=$a_!FVz@(pBWdeS?jCbe`pcW&%1R_$5sY0Sjm&gqTDto7<|3A9kI-siV2^W@gC509R zsaG0RdJy)kmla$6A=+?nX*z_trp)7;b!DPiC6&XshP*nB`QPtU>Hn5_1;OeRzT3&h zyuF0Ey+{&_{lF#aY9dF}*~KoUFj-xIi9x8$)CQ2)@Sl8in~WF2oAfiH-!0;XGNtBx zR|{LJXyQ7AhwjNoDzsEYAI|q@y#Vx{CO;CX+VIIF%RNfeK=onp=;YulfH~+&N;T?{ zUYF|N6JC$KL^l|$dZk)##t%h{KWRHK>(8U=IdeK*1D4q|Xsn_NCV&ss$g{{+W`G3fL*{f=7?#CodWv>DsYPngpFG7+nTs!eZN`xld1+DA6hMC+OV+a-X7 z@>C_&{kW>vRurJhW`YfRk?H0Gf9<+;kkvDokwa2=8p1%}93XT0Ks6`|s>Nghh^es$ zBo`1{1@pQ5TL0Jns#$NyyNA3I#gtDZ z8gE7XVI$=zt9VklcmlIqx%C*?RGuhm`4txn5 zcsd_*G{pM>*uBka%FG5dz8);p2SllOjg)Xz#@R{mB!RllsLF%8 zaMuqiHCw!60q+}Non>FlFnzvfO5&yQ>dd#_WyLLabD`~MZ?F^tr!2b1)PrNCxF4UI z!qG5mXtlB}gtNRSkWN_(f}X~#^<;-*hr)D6g(<+>g!>wf~nin^C72snSqwX!+jyitLqPs3-`67y*gKHsB8{=C4O5QT_*>SS6cN z6p;r|jA$YiduQCkd-t2;TQ&clnyipHY&D2DPdJI$fr47qNPf4#N*9Hi+_+5t6>Im% z&p9{_j-MTGHLfd|5aj8W$mNyGOa5>yurKa@7b@!`szCvfj4P(rjKzK_i@Frz8bKkUc-#*P*cmP} z{Kxn&OMOXne-p?PytiEd*(ydAOs>$udo6X>Kp9>Hyr|vC1`o5JCxV_m7Bw%F3HeFt z86+k{F!T;zx$c;D1U)4Nuxb0f)|~Zq2WbRM?<5ChBxP-gGAL(~z>p$HN6o#9<3N+I z{0JAEz@{@#hakGu6Z4eNG0DG*V8)Whm+9A++RFd?8$r4meZp2>yfgC%EwNhgSkWkh z=y;DNvvcZ`Z49kePKNcX28Xq8*5nx>jY}m;oX)2nm>3G#=GjiLlDh?aH59y6!a*)S z*aJ`k5K$s?f<@P-Qu&0ba2d9}oc#UlOhP@;KV)(Sl`G*R)#ihIRkViO_QvKv`6`nB z$Jt)Qb!<`5WG51sac~^>u}>-s4UGm`R`VTNytY>X`)+`a5~iKy#oG*Gr7ko)O;9wX zFNvsAt+x-oOkiQ>ed0{UBAhLoh#pfgDF}{(UePZ8isbaESWznY2B6_65yBUiOBE1u zLeIPW zneW?utu+*e{#KS;i6n6L`Dq?M(S-wv-+WW*aTzY!oMVI#tx%8+>) zp(g=8Cqh7HjPh6jabe^Nr4L`a8@Ldx>)U)xlyh*Tu5Czz>0EgnXM8A%5tsV^q@VyI z2@swVFA@g_Q5DBACB41bbnuyrSUMQf*X$LQoG3^-_D8o(to_Uid@VU_?sj;s+o^{w5F568BuZ3*zA?#8?-aG#anSc7LFeZ3E zk9te1IMF5X67^IbALB9XK$zaYWw)B*@vss#{jYrG)wQl+el0y@)qSxJu}c`FfXDp^ z7hnJVWBx|44Ie&(V%SNR^>3Az!wW}v6R#uNS^bFB2Nh4)sJMK|vBLTrb(Crtxz-USyG zDdh2)=}Sk=e?LFE^=pEZ$Pmcsx8XbWYEjFN1KL-PGZO@gaQ@_gq?5T3#Q)@Uz0i!> z1?2jq9DLHL(ELjaT|K%F5C;EM3jE8r6%Ia1I2!=Dgo}%_wXqQR^aN=Nj6Z;ZE-iRQ ze6RHL=2UtJ>QGW@5Bxev&Hv-CJfC3!0v-8hV>i|y#L5(HNKyhOGu`piiizDImUVXmB>ZHxBmR`5*4+S0@>yX-(%w75A6Hs&#C$uEuDD!FGP= zfp6||2SwTxv&knOuGSFqz7Q{d7v6P;Q>bsw?Q*}pzaw-bnCwm*erh_J&AsDPEkFMo z#O>%-b6#}0aw{{LVwb0+sK;yFeloTi(_1|O`WwBOUK+frloA;yNmmbqo>8ujnKrqM z))p(p?QxU)vF7LFT&4$w7?2yKyMCW7lSt82eZ{5*`BbDG zaQ53c(Pjs4K+RZo*uD4Uig9O(!vjQhD0)I{SRydmflg z7Qg&#OiBAME`T}vnd032^v7iD-4n|34?75@AHQg3j>pL9>HShzLgR1#((YkaDW7IY zqab6OFR5=CQqO&kQuwvO_V*}4@ZloD}dtkG}Fo^I-aulEX7$4*0&$yW` zo?t6(Y!S59=$Ntl(3N4oQDB}BayWwU8XSi?G>oOnY5cBgb)~EGcl>ZB-Kfc+m&7i| z%E?ch_%~@7dzD+Qm+yDSK}lwak^UL4;)hL>OGFaEA79OP_m`TG>2hSfw5VLA)em#a zvckFX>famnrV!yMixRgS?_9BlcZyd081^2oXYld;nx=yzm(>@I3}*3n4OSQ0*o!^V z=5>9HO(RPw*;?pol_eeZI8OEjoSH_;d?Vd_mF;?87*%p3jlQ(Kmj55ntX#^YQ+V%W zWAaQ`U@j!A!W!w}aAe)86;_D0fyJd42AB&yt#_%jNb2rp3gSMl(rY4X+)o`H!_?c8W7aL1w-PTfv%T(V~I{54NA@1o(Q4q%Al0m}~N0xM= zvX4${@5XeUE=2uw&wCFYeIyXkJTuOZklI&w=j|5|==3&lv3ADYpg7DAyB{%E|E@H@ zez&(!1rhCw6+8U$aA{5(w|sNKoD~y?oICfRx$ z9O}mzTxE%>v`*X*09#9kUIjC8X#va!;t zZ(g8074%db>`9pt+`pE838A)7QmG}#a4$uUk}uLkh;c=%H4O~+!x%@&7?8r(@2jB9 zTR-sRxI^4Fkg=`J zH#|Gqfs!*=8zpZ7q!1z(~goYGs=cuKMU3$Qp3RmriSmb8K+y=K-U$If zXyR0?H`|7rv)!jfXIdV-+_%T;BW@A<%u+Q;dE1kZWHhe)zbHkd zP*%$*M*}V$6f$eLAj-LX_S}ArHnu-8urO|LWnpegJa!(RGMO&T;*c~$%V7h+X-@y9 zv84S_N*DDm_{HtS78q;YHhiB9KmrN2kBhQwHh_eGYbA~`{N3$wm~Wip%b1+ZMt3j( zr~exZf#B;B5;H}@5%)vE2jOefgyq`7r+_#ld^6}alK=9HeRO4PRes+pDSii>C-Gzr z9%P}Ja!@^7|FN=kV(^_i;fPzmQ*@CkR5-{%SsWbk^$sRk-qYxpH*Jo+9i~HeX&&pH zYuzF!pL3phJr{nA^qP9M$6_r|1SMq^`=i!(dKNA%ZaWm>_8%PtbV`}{Z_o(yRjoh= zixdV?Sah42=EWdVgFRNVSg@~D^$x9tW9rh|q9l&z*@f?frxXmT)TW@lfS#Oyd`60% z_M03{s5QQ`9k5Q90W%iLrdjP!?nx|tR8$n{!7*h&P;DzCozSQ#$`ehkg)Gc>?XJ|IKz8<45cm z?e0bCtS~qXAQbX|Qh*+UQvv}hV6Yf`ER>cdo`3?1yCI+e*m!@dhD6X9gLmf$FbO$t z;LX6cM{r%cLVx#u`)JTo^&Aa?s<@>@JFNF?)Hk z9n~8!J@)vlJ9|xRWo8iZZ0=U!!S)Hl8>P1twlo=|H%m0s>NQE1YEe$ApDsnRP~{_? zNcv#ao<%-nqvayhWZaP`O$jq(6YeC4|R_PFJpwokJ_>EJ-b z8wYh#l+oGjFvGJn7sLFcJOzRPY|5_VzOZt~Tz=oP!{BcYgu4A%R~Zb~9-PLeRx3us z<=p~Vl{Yxv`I-Pf@d^$9rGYnu z=vzjOs{uT}v&R)Hl?dP{Xz{xn5>FOueg#=>x_di*#(r=1$?{ZbDyPGK{;!uIdMd3{ z{Tj{BDU|}=}2E@Nzb!a4r};XHI-wVSPGK~(~aCEsoaIO z%#a|zXQUg634gk-bSsohz=7Fd7Klbzl2>El6CT>P`!$8Ao$;N*ARV>2`e7EF#7y|{ zEK!6zPN7~rh9+7~l?FYvB`mGA@E4RI18*Kxgd43%*&tLR^Iq#k1i6a#2SAT`NBJ}d zAkas5b1JGDb4V~J0QaYmiq-T-E~Jq7s;kCh6{voEwAOuLS}JvS1q{$&5)XZccpDjI z*@p#28?(ED&UiLs=7v)OubN~Rjd6mo{kDoftAZm|BDhY*raLSBVxe3sOOvQsH(<5N zD-UeN>|_@75RG!GUUq@RPqR&9GuW&(dHm4_EBhMH7HeX9^x3#|<1n#t`Q@{u)lL&Q z*%jvu73%C)IUx(^*e}(^P^&_n+AAb*tW68~d#64L`;mTO;V*ac-=B(|GlA`r0PfRf z@X3W_%wLR;lyGrqgY#OrbSsw-HqVd8 z;?VcX;g;EOAmvG-V=Df|?O|%l^yY=hgP?@kfAMVUxJBuwO0`A61LVuiEu~ZT=@_Fd zMuG##Be*O+Ik9~VuuPm>9%*+4Ec<7GBV1&i5Gu(dKIDIJ9kLrYQlQ7K5L1K+=PM%s z(AbU^xm3>JkNX5|1DP~$LsL6TJt2QcoNi5>MjDZiw0J(jk0D|RocYRFe;$&Z&XM*B zuy{>-DM|^sKmN#wE*^A%Yi7OvN)*1;fyAaqsU32**E+qG~U=igGR4Z^9$^D#d2yXfIJ=1PWV<1kln=L9mq0 z{h9S)LUAEsFOI{ZXzhkL<>j$3n0-cI0W($&26iyr?$!_q9yEO!G$I(zL_i_X*4KF%A9oq=#4MGg)NmgU+16&;YYN32S#L9UjZRYQo!&Nv#PZK4EvwkbNVWKj zAS;o~rd4CRjbp?w$1$9^bR2LI)luW=aT5jk7BdEvjcfoJaKi-{#=1YEu!#oQQ}SL( zHv4|Im4{Nz^TS17!0My$?uKyw@cWZloVmx5W6+0WonT4g`Pz#H|dbaf>3;nsGG0HPLdwag7e={26s z!vVtzhvKvGHqaEe<|F zx1?Bd@BPYVl|am5GC;Z+%rS148`c@vSxFAcv#?^$k-5Qyf%_Vh=|gEFv?lD~7GnfU zS4V$2X@x~kep==$R=$n{RD!KknC=~_r9yM$nfb8>KMFoK6AFb%sd3YM^2-|0?M4B@?Upd7V z7?ET+U7lP;O|L6aWv8rB1;Wi&(TpPZJ7~CMDu_*#!i?TY-4pqd!)W;Q#5d{LW!d46 z^ZKD@a#t86THB$ZA;X+80XnmI><@f3+YKxmoDK8Ix1hx#W=@UAl+^;n8S%u>`uywM zO-?efBEFaygGO{@^~L5qg6+nZ@=fnf1sC|g+dLG>^~E=;j-o(GvD5^~Xx0$c*-VC#0BdPD#+?Lw({n z8@%z)&NwTdVqC*^?*9QIr4s-*Tq%XOZ@aNP5Yd4(ZC_$wU2VD1i31b~$p(4E!{l?ZQs-Wf4Mq=tOUU53MYp0cItiDk?eR^Y)_&Yp+9IkUBr&x|IHuZ z@xzCyHh~%ofSF?=379#7sxnh@5N?n9vF;}p!WD)!)S6WkE~mr+pu!bE<%jXhD|wE? zwm=!jt$IL`$G%iW3Lb}!_})0<_m71?1=U|M;H|%}u6OAN2z&_eQ{5@~$wo(nEf)KD zh^lzCY6&+}A@hSf9|fbV38^v0Yr!XuxCBvRb1e~dlfzR=p6aOjy{vir|+qRaL z;EA8b9hR7~U#+e=P%eMf4AQ5z%(pZVgNt7d)oiGVp5B381yCI2Wp+Lm!1qhBr?qq9 zWZCLLPE98dXLp3uzxb!|pa|M{u)EEk*aooW#Qb&74y-@;susKRF^OaAKWz>H0AW&H zElOOB2W<|fQI~U02o``g zIt1)BQdXGvf4VV1L;C-#8w234`2I0|9|WO1pK_q}=>tAO_5CSfb zBu`zbv;ZTFaFa0j(Lt2I6~TBCpp{gdX87P3r2ooI8AA}Tmq}SY;{p5%8XrhFLE}Sw z#Cqc&k`cbFK$H`80(apDp{)bHNv9!ix~JrKL~&3ytY_CvJ?vkilc^k_Hv}Q>t2~(k z-8!ImTo~Lo&9^?(O~BP@nY1~;AB4yyQqKszngq)D0wLuL%Ie?Wc)%K_fCO&zlO;}c z-pH{n0$S$(3=sHN&p&9xNc5mOCh?Q~W;jW`7LNn7Rcg@`0zgd>O~HWwf#AzgD*-$& zKm|S+baXR5FBydHRCzf8FbRoYnoaz=%KnZXbaUoX*HP%@4y!F@L_wvlFfq@Qz*s1p`bk%U3qSU~N@7V7ONGBvhF8 zn(c82<+QPfZxL(6z$^~T+iN1~MLi)YM=&GqQEiH^`PbgROS$Sf_0KSd|5?iy$7M$H zh{5VDPvRi(PfXq6X48jQzVNtQy}fOeY-`F>?12$hMl}8ZK1KLNRnIF7mqf;o{(ls+ z3A!lk-@liiwwmp=xJaS))VepX0} zDH)QicbtZWr2@t@Sa=}E<3hJJyVkrKw1x_l61%sddTV;1l>gb{&Zzd%j}@*3EHI<^^Ng>9=cBjUc=e~Mk1-; z?PZx%1nSVDZvbMXN1l2QOaL|oXowyX>B`n=`C_q$`oCgFwHYP@rd0;Y?t+>}ZCKk* z@8-9F7^p+RkVzzL7+8)P$s;L6sq639Q`Fg>xtzOV@%r%Em8~GVgaYy!{B{Mh;b1gh zlc@0P%B!1n2_D?O1cdZMet-L-(@Ktw)w`Djg&_EVzW2tzCHqQx0!Z7+0vG^Te6iNi zj`nj>qVltOcKTUIY;aqrE35G${|@@nzA zptL{CfYvPx;vYKmEjUA?9l}57)kHl`qrhn|{1!@C2Fb+a6;~7x4b?H>)I%XfYD*}T$5U~XhXApqfx-&dZbQwZD zNEb;#ds%IfQ!CVLfcNf%OEA4dbZE_DK(r?N3CIx1tMVR$0#!M68joraIruUE zPVY_zItVjZEP6 zyz4&)mGNU3sahV)0-+g}bhG+Zuh|4{jN|jQr`0BJObrr1b;{t_<~?*<37`T43)1>_ z%8ptuF|I}5ZSjp(JN1rTd2?i4xfy&o7+6`^G9=-?^V$$|=H z2qgS5o8k3_b#K5OuPDI~5)47fnVr>eI55{aVsi2^nDhHIS8gs>mQ=CM{qjVHZXC?e z`q2?{4^}J$qum&GQ@R!N5Yt41WecwZ2uWIi1E`Kb1%>jng(CD5LutRIl@%fJQ>}wA z4LRYFN?B3dfrRIGZneus(_$f?L3Ah5fftPg*rM&N2_9U3%Y^jG@sSL)Ce9oR`a~Njp|4fMak5$^M$*S$_fbP6B zBJf77fe?mrFj{Y~$q5eVng^JKiE=D}TZs2bAW?Gg=-WXd7VZD)9Mqu<&*`uF0)-n6 z1xu^Dn}+_Z;>{yZxs*aY44=w5gLh-IU?gJ1+dO&*8`&xt^x=fY{D?--4Y@sI%34+ z2zmP3qm30xz1IQ@zvUTqWDR8#)$bBt47jY1I_&W}B48PFz;Cok%LnHmJu9Dy%9CgX z1Iukj$Tr#%T*DC@Sd;Uxr^re5hIG(hFfO(B-gVpDxLa4{9o9I(ok`;VFnSCH1;Xqc?-AVzu$LM!wxZfsgos6QyvVx76$$ZY9~WIc4C3y}HisqPd0q4s8eBy04Xj@~WrIoa_nHAp zuRrd_(2q$xg6S28UG6-g-VoGD8B8JAXq(Z9KdWSiAzviz^)A*J5gZMp#h8LSBwfGy z(uXy_oYTx>ES@Ym@m55C{zS%{p^B!5#ZTwDTC4_jyUcJ%q5s-Zr- z!re_&xO}iy16Pz;2pq z=!d$CU&txCZ(}hmwjOGn>s~dl z^+b2UwS1pI7!qnw67WFey5`0pE2i76xzxJ6J zgn}zUFPX-_up8=)8?sQUU$+`XLPGd+lp*{4SN7M~r=K$;JHcB? zZp3OLFSvgzHfCByArrcQ@W0RP?LZ~tX1P&dGvQ#rA!VU(n0CGLKRzoDB>M~_E zM)MRXQq^rKrAr8PIJ7z zuz59>d^e&B)V-dKUDc{xe}ef0ucRrf**yaGHIAhLkEE3Lqc^~WjZ06|_HnEpWfP(d zV|<@ozF&L!tnqTAqBao0cIA8Q+3;0&!Z$7d|Cp~dLU7lHVC`U5OgQBU2{s7}30#oe z(;{bv?V|0mf;)_p_Yajm7B*Y%%v5KV7>w4K}l$Pf{GK9ajFs{zp-5cN*PoPgw-y{t-O6bn2W@?$N?ppsB7hqG;Ukc(f zng10XL|=gqTEX8c6^nV}NLXog94tyWlK5Tly)xtOj_B!85h`8td0)s0w;zo(O>kbA z=ZKC{$IKBL{oa&!_L`e!+aKgcnYG};5U@4qfxD-^QfL{%R|yaweYcGFGR2Bd^2;8`)1>$ zFcV|xN*3v^p6Lz-NJS&|T@xEbJwgr_5JHlNP-7r(^W*atQesrVOUP30Xw?R)L+q|( zF?}D(*|eXVDx||d<|r!ZE&qK=>Cj~HS&&1o2GoiPh*gw|J+a5jEDi1i*f|IO{#H3Zsx-HYQu+BV?L?~tC4@Fe&7T|D*&l1pOf>tP zT-g=40{@|M8&Mfo$P0&!AN&hJZI|H&(a@KyhWRi##r*l`)|06fFh&vs-tevcoE+66p{jJ@7!1DFhv zW}9s%FCSbp-fmRnDO|M(UVD-^b6W3F(8kh)dR^I)3pf12P7H7AcbimnW`3=cd2 zAat7yFYW<#`9|`kxem>7DEN@;Oz|xQ<$$Wuirzv7^nU5w{N2U`-Wsn__hS$ zNrPlM2f0?W#UE{BnKmmrw)U6s^W88Nyw@2Bi+I7%o3iIp?{4!1fei{sH(zPmIi=?E z;@G+O*~q-kM;kmG(mtMb!^>eaM%zaps>&z7VD71)xH|cAdV=)zDg65NGhHN`($Rm(abMTu`mI#896o3FL4nc%H!in+ztG1p3L(CL^k2 z8PpUp$IPS^T3QoB9eM4K8{T-*n47AXJX`#ziF1zu$(Q-|%KJqeZFfj7m3hKGI2RoM zp3iAClB|=k)}Xo~Jo}j~{BD}g(}KF+OZ(a9%sp87Vr%iv%$*T9x$EmRV=N3@GCRrq zvESbFMe+1xyV`EY&PR}TH!@B;Kl@_IkI8^Gc0xWw&#S-5QaPH-6CXmul2Lrlqxszi z2Qln=^)@^1b>&o{8UkJ)er=P7GM}Jx8TJce)@TnbIAUSCj(+i0UD&rx!c7!|%Wtcv z_D&-ax$sYSKQfHVR0-!WDewNK>@i--hkKKsXr5cLve&Era zg>^tJFq89|ry04+ohdCwjJtstD%EMe#5%d%*l?zD4#!D82j|ek&Q$0Ml}_$_^bgxC z=>jD=^2*q=EuBvv3qn2i?|-Kn|6WZ&tW?PNuZcudqA^4sizRZM&}po))Sq5N>h(6d z^C-vh10S2(k^kI|62<`O88kKxHj`=`62)*1=Y_SM4eQ75`AKgpBsEU*64`CGyj&fd zQlC^T@EcB%T8*L0TVuNQ=$A8JU#L7A&B7;$V<d{6=&Y8jCvUS*7JuBr^uQYWJ zHdi7R3`m z_UwC_VvZcCS>aqJ<<}piF!dtFl0*dfZg&q&_4R1e=C5y7MZY#E%vDwOQ()W->I5{c z_q{kMA?ThwA<>k9jbPLwXj&+YBF&@0@YOY@LZ$#A_)AaG^)(9JDfnO~KWdCm3kbn< zNstp=f-h)=5iMjC=MxQP;yXo6}_>ky48pk0V|@SFJ5o zR!;s#af55v;srk=e%Inj6u~Be{khr+&T!chr6S+?bJ%2fBcbtKBZk!Zi;!DO)v?(f zPGz+QiVNoW$M?3qQS)e|&a$^RIv==f7-gNqiJNz3Ul^TS>OS}sUfPa)Ini&}667fU z!t%wTDzVZiXFX_8CW7O=BQp(>J)DUwehUM@5;N)b=@FizYIQX*J{8 zEfc9Isgh$v#`fC>e|WFrRvt%kLkSDv^GZxid@~__P!zr)m_F1#(RJV;cCR5xc}y71 z7@RI+QFvJ~^n>XaA4|@$Kmf4H^xplFJ+_&7iQU=mOP?(AKgP#KCCV<vC=>Hz?o zmc;OFvLCbKO`CX8K2KP=)# zBPNm?KYoMBtiPNo{3-z+d71%+oKZcCXWH^Np+?!)9&c35FWXasXdhk{&3hF$+vvP7 zAsFvFp{)1VNluU=)#+G!0-SDE0^@s0b{u&=9ohBM{=86~7=_a)lk_@puuOO9rQTXO zt0b-d6Q8E>xF`tsLH}TP23C*Sbl?+0y5Akc`Ev&etTUsFIi#cW2oxKmMgw z#Jxvo$t0qAf*CdDuhvIqg1yd0r)K=VQABY4ygyIY48@%wBbK(9YdxqvuB1g7ubyA{ zJhrFk>pV97sND?zH7|D8Ip@v50JU>C+W7CdVqxVW>0b-gAp@TBkPo5p&wmMcoTR~t zPzl0vPzgAfz0#$UgQZwjg%6HIOxGtE4ueciZH7~0aESGz+5=lX3_})00z-(C#xwnl zIqD@0wlzI$G@{#FP+Z4$-`I>&CJ(3UlS=nN@YH40;+{9*kC1=8Z54NBw)_SeFU%aU zO5W`E{;2g8c_#ePMw_p7LX{LI^J<1@ueX#vrE(;^c&jGt+urv1Yzym!!y3{HUU^9W zK1^_2JP45kBvPHZ;r?d$tf#n>p77!T+y#NNe8Jbc4-!xu;2W^TCMG@C^&EdMv$%LQ znF23P@)(AELvO5x_XwjY=(@Uq#3Htz!}7@>&onL*&Qt)@f+q*|8j80v{@wU{729gm z*d(4EWV%^A&(y?vy(6j;;;P|;pIxeIaPW!2h6=O-@bY9CiPzJJ0tTBo_z}P%#x-lQ znB+|42K+t4Dp?KhG!y@(x79b#EWn~-dV`CW7zLe&<8mu=jXPSqXP#HMm&p8m(yUu> zHCRq@5$8G_%&+0$@`lI#U*Fl@)T6=0G0b2xMoorz=m=om-omY{Z6Fg99>R?l4@>*Q z@A*I4NFNiJcrm%y>~hvM;D6OOS)Y9*PPC#`R+dOq%@8C6OQD+!ADra8tWexCYsREo z4nhs_)>i;iqQ(fFu~Re0w$_CTkua<;_H^UtIP||s_`qL!;g&G5NnpIem8S@@;Jsfe zk;8yL^a}I92P#1>eSvhJNrU~vWZmoC%#+Mpy`cf}wi;g3_l>l(kR=V{!cN+I*l77j5#klUf z-C>U5Ixq#+_WNY=1{nk;@ueW908;t^hFC>LL-xY>c_{WlM z{rM;q7?T)8S93!kcqNNz6*VBwX_XK)0_@?qKcW&`;xI4=K_|K_K(%ik0}GH+p1BD; zLASz8**t|VI@B)MBRexAa;$-G4;Od`%hgo*7BtSz=fp`7I7=k)Rx@t2rKq+%=&oEE{bj8sQs`2(YpY7$w6VMdSeKTf<)N zhm+H=onN#_j8uYrXphN{^hg2U<}zxi4&IDH7Y3SGw|O0`Vk^*KFpl&WWug58*ZHFL zWJ&y&?81M6`}VLX!kM9q8}0YC8Y`6wD!;-%qE~i=ymT7|>lu|{P%SQi#k@+C!BbWl z0sSsAtm!ICj9a+F4EJK%o^O9?lW1l6^LWI* z*{|i$pKm1vkAG1yqggTx#Gr{zOSh-^*Ml-nWUH!y-&dj3jr42E?t31%d{W*{%Rk8T zEO0@Ol~sRvt;|~08k^bsui^yz>f%Dz&DtI;p_rB66m<*G)G3mm>J7Ai^}Fz#aWWC? z))C}+WF#SRaq0>uW1)g*nh)xbUT>LW7Mo^LUX}SP>5veiHoaC{Nkq8ZGtD z<#m5@w9njNUnRvq+VDJC?pR2C57a{(9;!LQWcJ*~5oy(eW$F0%s{9cZz8dT~0NW*jTO5FBO~SHnv)CJ?3fBy*lz&3$;oRrWL*9HZv?%d_W|jPiJhPX9 zUdKp^LBpdc=Jb^Q!x)o)$&9F)1-3OGtelrO`GMw}!?(`Ext25-*Vgti?|@F!p-4@R zvH#T^%}T5CN$~wP;QN2gu>;*bu|FRN7jbe6PYzZ8b5Ubh`8l}g6_;$-e}}99Io;Ua%LfSQcONx=6X2h$eU%M~ zfK2Q`Dm9Eo39NE7V+25lDZeGeRFVGU*2`cL*r2)wz`I{_#z}t-GKcXFy+iw{iq+XV z+SuUYbWy4d(lgR!5FD@(;Q%P#`W=v~n?@+Gc5)_i#S4fAHk&(W{kQh;w0KN=|FU$^ zezI_Q<0I|Y>IhTz?QBc|l_&P*Tn*y(6r@(GdSttGR6}!U6XmY5y~Ut4 zm~+)3ophp0{n2l5Z)NRe-t3mvd&DK5mN)iVs!rJAP#4=PtbfM4DFw?Y1@)%bz<7wy z2Bc06M&+)!UVWn#4``}a_%0IB!4&-#S=u{Axw>#QvDS6Q%~?$VyG#uF0B_!Nsn`Jc zBv1_yT-w~|2Nt_pv2l~wBRwj|VCv`CNQN?#0&WZHZkVRFT2-WY^wNetomvI%?K({y zJ*DImW+f9+XtKsBrDM!1-l~U05z90G z^_(Q_l<7CefV#e~_u7a9Z@oWe_Zf?O!R%J68e*db1i#1gj~1biYpK1%wnm2yAjQ~X z)b9K?Fc(25_KB@n1t(uIXPAg;L(nnnQ`~a=^Dis46NIY_Qy!ZkM=~u3pHx24gzq_r zhYzA}bTN9C%Tr)Jf$Ezl(_G2vVvA>j zcin5rE$GQ2NQ=)Eb6l!v$w34XF=RPgg9zh!krC18vPLyOKaag%Md_V5LRr~}b8^z> z_LF8WOYnOL z^q=^kiK` zJ?vdmcblShupF%MA5+=?iwZd`lGd4pjb00Wsp2V1MYS^3_R1u^@nqG7#m;j#O+DRXmE; zhhQF_AFZw0qj4G_oE#8dZ!k!Ly)WPKt4lV@+QO!T^=Tk6Uy8|ZdlT|eHBR(o{y3(quh9U1QjQvce|UTiCbxd8a-s}Ef6%9W@# zqRVDd%9)jyf?`X*b!~jEfb(dz02T_Ejuw3Oz|HpMuIg}(i9WP%EV@8Ho<~yJh+U88 zljx98CVk%yv+3g?U0%sl?OTr^t0y>BW=K-GgTRnn{$%ArMicHD&krY48|~RLwB~c~ zH0F5{LUn&ptV#Bm|MWxV;~P04p>op; zERYGKUu2**eP_)U132eMjx7x(1xV`KraMbhQOJ(n_a+{*aFbNx+12cKr&Pv~i;_E! zQaELbklL3hpsS5$w|T!DVz%$nuY7z3iYIFqZ42YNriq6Lw+%K=!4Pu%V#e&>m%4_1 z4F%1vgIaA7bG#j-BaOSTly3B; z{zgvNAd^%Kzc>mKs%X_8t9`r}!tVEIjc!9CQhDmfWbr(oJr)*!hnK1UHUqkg^Cv_w zw(@rDI*p%ifitu&yAEf{v7CNxqJ2{RMe`!IC*>6p6u8ECP{(69Sf*kh$@=h$2h=1+ zz7#96!rCCH(Vm)hckP&5ReU*_EjCs7@)zM+l)jHR5pedW zC_DPT2u^5cyT3HCVxtxPT%j*CG$~2TI3Id3el8Bl5wa2c4uTi=DcG~8e z3{*DLe-?Z?hIMLr&#Q^i6uq$TCDzNg*X=X_>4Nx|pb6UWZl>{=P44tu^o8e(qT zfqUX&q+I!i($1CKz#M~8e6%JpG+CiiAcq8&^RAe&6i*>G`}=E)dMaTR|K$Q!jKXfz zj%Ww&vV)#{(*gRMkb8nfIN||~5CfH4`U9{eK@rE$h#b2NcxBHMU5BlJLqWA13Z3Uh zsXwx+=f*d6?S5QBib>%f0A8rKb4|1p0S*sZ+wWpOI{L}l#AO!xo-kgk_z83SC~977vokE5sk& zeK>pyp+_}*ol~jyLE#3*du;K8VG!P;()W~2|B9Ivow7oY7*#Jf26Y}+zXo09=z$`K zKke3Te?V~F#}RCd6e}ZC|8zP!jng?0t8#*Kkiu8I!EV-WcqM4R5mfhohs@iUE zR6hL>opIo~AS_%{(_AtnD1mHO1~2K_DM}c&JQJ&=RW2peSdtty-Ly#2P{T z4tToq%Z;N^C%veb;)c0s{%Py$)yy=WN>`FiVP-X&WIt?HzdFQ1HOj$ymwyop($@z!fk%@k`p^cCR z_gN-O&F|$pY$rRVip8;V;kB_?#qoQ{uh;kFpJ1E2FlojG3rs((uRLaR{jg#^CV#m{ z+_YXr*pq9jSx^rx88uW#nUC9H3JdnuC(SC2C%-eJa>b%i4f_*;zzBaNgL>O)?;N!^px-;8B> zUwZLItg%2Et7=!ceW*cZwD&z@Cmy@@CLI^b%^e>F9)o#~7G=C33NMppwBt37i3Vj+ z>8}`6D+a6Li4}>V^IT2 zpMx*Dmo8S+FCLfK|KiM6986K~GzX{2%=C-XH&Ph>^x(lO6o@H(_}BL&51$ zZ#*gS(086ZFPXIrI_x(BU%rc*GnFH*iihF&0dp87!VTJgQJZh@!lUq3Tj;fRp1rg= z6LQJXAQ0YCdzDRzm3Z*xQ{zU$m;_<*GQbw}VBF;7teS?o@n)T->(1z-x30U(r#}L5`31~>NR!kyl z43kDDS$Xl#u8nBcsou6t#c8L*(;zBDxCl`*g^VBN* z(y$OGUI^1hhZL*l(M!XQRpuH2aV3hi`;*gx0zxl~_)QZg?tFh?L%+Sj_3fZiP^KNCoNYx|Hx9zVZ z*QH{_Mz`cWG~ZPEHh1|C113{{A+~` zPIjenSW8h^WH(GbQ3{8Y@lK&)T}?hCMO1kN{rTZt*QRrumJAY!N}oUMc;B9H%0M{` zAPNHAcv-B^4D7x4hAKl}d(rg91_PYSH`S9jIeJe%*G9KLvY+=0Ny%o<{dDz)yQ~KA zdN#&iFl(y+d-xpS&4n$=ijVzK-W)g3->p1sMt3!}&=W`LH*~i_s9~V0d`4uNPPO&j zZ2xvN3hTfZA^vyujX{doBj?GemAlzw*3qu#z-g9~Twp?)DK7+Ey6TDEoZmk?b2*J% zcHX=B{o3KVlI{l^zaYlO_SosFz{oH5v;p69s42ak(SAx0-+cpIty<-z#fsTfU5Zn0 zuH1S#yH<5KgkI&%(<*JtRAi~FMhgC(tEC8YrwCQZl1X{7u2pDTz~?7V8%aOu$VyG1 zow8o(-e)~c|}}F=_<#EhKHFE(QCtvGBq%e z$}TTvGn`O6iQ#6wM{0}u?6>9P)WdYCkR~|M^M$KX;0UlVRut;#u!csKoXtFjL2Opc z%nltmL)cO}WUx-1ExJOQ7dnUpY-%p&69N`~KkNFnLADuHJ&Nk{V=cm=+zV$YQvqQz zXSUIrxbqNl@$Gi{m%f(Ilgpo`r7b#27E$7jWM6BI{!qDoh}e+N-lm8kj%MTO$$v1+ zc7@8=#gRh&h@ynvEG$=;1kZH834n0M8`Ha4`%CA=&2de0e>Gb@7>S}n2lPu1kHIfq zer-j*fUy&6X5P&qlvZx$F-=W@;ycXBEUfKJlmpS?$qxXdljlbm>RIMS=TcyxkupPj zA(;Y=&_~9ghlQBR$LgwDxrGjqK8Hc*`f7yzfRsjnr;g zMoG{p8VfUx_lu&4(wzY-ysm4{g)n{JSN3g^mfnfWLsn9*=HDt=Lx-wYlO1=}DZ;1t zR^GJ0TycJ$S6~n9NqgS}A2wHo!QclU;qZfr`l>E+WPm3s;5~T}G5NN6L1uH0b3gXv z63?_m5U8LtjTB@yiKZwfVr|xzdX))L579Xnuzd;Ml{2bR7RtQfF^Y3MaHGs}m@8~7 z?;t2OlMQ=mN0PHDf@iA2TE!VGgso{fZ$`-`&WqDS*%4JS0RVnt*(9rK#B}<^)uJAU zYWk&d0-sQUmr%v3XDGuV&fa@R?`pAcGhKY6H3F-zqxf>sejnczJ(;B@ird6siQcMf z^-WvSsKyE5B)a#xmK|oi=L^BKUPJPlM8P7`EV>j(vFUPufDvZO56KiyI`LVK*}T*w zDZI)kneLS?6>1ANy4P=Li_n6viy66@4appM^1c$0HF>Je7E?J9-OE^M;l5J&W+~3? zpwhFpUa=znzTWxd4H2gadQnsDfl%f1uEAy?Y-g}B$6!Cy@&9l3uMi{DLg6z#sQc-C z`z3;XveZ=k9gA`V!mY;q(S-Z%2kk13o^&m96w_bNhhC&`3cO1bQtgQVzR<#<$l}d4 zWC4;&&&c{GD=Cjbxx;sLb)>t(o_Q$8rG(;aE@kwx#H4X-@9=6$^ZlE)$0O)8Ta?ps z{m*WX#`k}RmRTE7=;o*w+ymwjjKuFa8UUFzVmIm^?F8vya(`W3_=@D`8&ioSHO5Ey zi=mji+tOxSNy(DbDD{hYo@(*r5xBAr9ZD+^Nz z-x1=x>ce;+evw43g`IhWIc^HWbRkRJr9GJVpH2bOh{c=cOvPVh>1LR**7xFYM7(9j z1)kgYxmIGLo?3>9MZrNx;$6*nEma&vaM1~L8{Kn;s*?#sk)L{sH) zI=@+}7?a~Vv?7o*qTaUQIjTd?y?VtIKowWg@LTsXPrXl1t}It9&E`0EN@l;@hq*Xk z!E{%NZ!UJ)AC9gTiZ<7lQY_dOJJj!TG9-y*n;)YQF=A&a4hML;mR}SO>-?ql0qw2i zV4b+nyb=i-QHP(xv+hU08ZKxaJ8lj(N0~x%#aoF9>{8(LT2%z04A;HcC7H>%=n+WqI9@^4*z^N{`4FpSuZ0YnP_}aq7|}B2G*4-|4thiRk0=ipJ_guY z6vmsU86yLM$p>yyy*#F6R?UqNC9EA|`3&HGqq#x=*PG~bR2Hj!_!Ljp1+j<)JFYcb zNb7|Hm&mF(J}wOqsoQ1V0$qQB6y`GI_-|aMg}GzB0llrc@Z{ z%)W<_>vc)k+XK1GPl-W4oYBFoti{QF_Ru*@mx28F4)8J|EUPWldLI>lGc+2e?>1Mx z1EvAE^U+@w!m=a>EkPrs0ZpkujB%bQK^Js^YY+59GZt0AlcxyBAI;Tj=~MI7gzITk zt?=B87?dME<^vFt|9Y#Pc7YHFS@cfq+ymO$A>H$OmYSM+`?T{JFp^?`E@i;F0dvzAof>jl4}YV zkt|kBWn8tsNB7=%>Na-#+{Pq|1bxp1UHwuE)fNEF%fp&Kby2g~eAKlp70U_be00+F z!M=it(ktuO!+niYuMs~pG#-FtpI9S@t%%butak;<#*dMXGBujTiIEA^+~c!{NGk1N zJW>5tCKYI63L3@30h{j>3HDdW^JewcmKi0OO*N}&^ggH>{u4lk2EBWqKs|1+1+o-L zf+vR=zG~-(pL@@k_LVk_=CeNBp7pnar!w7S@|Er6Qe#iy>bGzwuM2;~_-YffrygY9 z6Cg*XaGytpQenx+GYIkPq!bZwt;4O$Zg9V)EG<;4!Rnb(s>o+gp8Ry<$t7Mp)Iu}) zI8%|Fmso17F{r|R7xG6>X;v10oiLxDH~3fk^kBOOAP$t9p(=uRy%9w29CMib)Sze> z$1)itKL@L!x@tLk%N3UumeY^rqGi6+sS0xgjwVx}fh%*yN%cmO8Sit*rMp9@X;;)@KKk%Lff6yd^%%VYwAzmyZ6`@h zs%z+RiSu-p<)P!rp;WoetNUp_T81-NFPIMQzOnsd)*{yo9OI1Gt)1mAxu%2fEX~^{Q5#nNp}Qj$q0F>R{M_P6ojh4dn~fHGwU{-l+9ll4(>=g&CIi zXS=!wjCW%|}JQV2WDa+O@7fK5b{OXzb zCFeq#Mj=5+_spZ1lj|y~Izi01u7k>)3^(N2?`~t;G=s4>f1JVQcLQh_06{1@IyE~0 z!ij7%2cnJ_#8w^0Yl4Fn=`OkhyFue8SGJN->$nNWVTOZBrUzpTqo($b`?=SJH;X~p zgPOyY&)(*vL(ucXj76e@MJ<;d0ogMT^vK|ecJWhwWRzf_DqQjZ{xp+xIaL$ofPDJEz4#>qc}(iT7YyXG+yQbM~q9IKB+H?Z3HA5x#v)gFpy(XjjW>TJV5lW2@M#y}Fn+&r@EYDWc@v zr|bUVsB&XsMI5iz;}$bZnfNm(u15W2fFjjka$ow!oQPw^CsQ$Agp<8t8DZ0isq9Rs zo&I6ggN1NIpSRKRMXgw`I}=ZlTibS}O(gwGPDneBf3OeqR{N-?+Xdq03FO@x_w=x`9L1r0f2wd_w^6ap)efM+a7RHZ981H1a1@o@p9os zVP7pAowEZb#I_@j5pE`!1^(v=Mb#2Y@EENT*Z(0RT!3fa#fHLP^Z}@k2~DpOZuqkg z5QXeQNwe$*ixcR<_a4B$XOvOa4Q{f*g<;1UrQaCfBNIUYyTz-Q$OP~4UoN}UOPU@a zDlasH(L9{-U=f7fOMtTyjin9Qv~KMCjw(Q|+?Lo+vr z-wJ&JyCpmFu(KCz#4o|%ThOT}p8tw8k`*HaGq2CVi3^hlOKjlFpzp3i5Du0BwzJ5x z3$GH-Jw49z=^PSlWW(WG``HvY7;N61Z z1Igg{z<)jrz&b{z)UbX4EH(cTAT?VLyIbK4%mc2Cuy{lyVBeU6TYGj4T8UuKE*Qb4 zb1|6X)`_lNL_8`oSb@7l><- z!6)G@7#yP3#}}~1X(JpF0S1oC6a#*t1sV^PO1*&Zi7?o*Teg)=fm=%AV4FSa>wEzp z=`z?^{7Sk%GQpj2ss9qfFqydzeEddc-(W5NHAN*3@Yn3n_RHx~D@~JAf2zUUG?cjD z4C3L6lmYymqc!M<=HbFfVQ)8l4#Myju!h;D_6vByK>NS|0Ni#LMz$(N*ND^)8ye3MT9i|nFsy`My&kjJ`^ks{1FJn~Vk-dj^lcfij>944 z;DDi1;i!{Q8_=y}QQ^Rj5C@<>?@Rua{dN4@B#mY==hdx1q{he-7SOS``nVqF=jfh( z2T1*T4G6fXktpa{qJ&*3NL->`*c-3BB$@eOPIpc6L^uQ#K#Bo;F};%;tjCvFF*PTZ zR-D&K?jG7Ed{yyGxXEIFgOYYdP11+VP#ec}(obr|=UDwlvVasUcVxqsvKayvYx{?~ z7Cl}`c=JeFV++MFz45Wd@?fJpd;4{rYi{kX>eHX%VUl`WI6A-SR@}QKp06Kh|Fzy> z;AIx+U#PN_FVS7>wMhy4Q{H83Y81?RlI`y9My~!)gVdJi1sqxUL~>ibvYqllu^RsV zSs`CBqZ`y3#fzeo4;N;*#;fQ*jjP!Rs1;^812AUYJtHW*Yh9rAFZKS~!r9c08?D+d zwgyOys?Y^sHaMRzFw4>UZvJjn9Piw^wb*i|zVOA?;ZNuFfr8pguGPg8Hc2SQQN>az z9hdl1IB(;}0Y@H*hahb85IIZXnuN>m20b9?^RM!>j<2p)yXWWmq3BV;x_renX5DI_ z&=FH(3cvn$9~E^one6{CCg&MT7fkbsdxCUhVc z?=M%SEi})OqhUdVUac}M27GRk>Yuq7*<<)j@e8P10r~Ol-&|@wSPoV)i;(gV*$JLD z%NX9KFR>UNGZ5VJAdMI4J?GscBjP_s;vJpIRZn_8)#1@15R1_;Cu{YCNq5uw0Ig;yfrA9vEdJl8;J zJLinDldJ(!#sp7PZ^a~$M;P^$erJyF&k&E+n5Z-(=iL*|AKIL*Abu_(fpaUQ*Y~`$ z^g9rUCE^s!dbdNt=YNHo1eYxQp5Jd_SRVMHYe+Uzg&*KqDIUA&I#6Oo`sLYHzOP@| zo)Mjo{^OCpBLsO{ss0nZUoR z`iS`@iXYqLdqR+V)vV#oi+f2Gn;|UPWhOwruO@x;_Eg`~a$(T2yWnZQn-cS_m!_STlji2m ztNw8JX7ZhkKQWv%3<7d2jc?&gYeeq9Eb}m3IX=sBAhRNgDx&3NM*FVqJ)_?@r3QM9 zp0=My*$1ocss{&G0UXd@)Q=bqFtG9-OU;YZlWY*?+D&I?Z|RvrZ!s~%rJ^|E1J28 zVd*RYA-)fI@F^LHD(to7(0fiLG#%GQo??2GJ;oqp4Xx?9dkOUub>k-JyCEm9J#EQR zZc=S^-ro16BZVFkw4d0-4-_iS%GOwFH6#c!T8(`>0hS2TDu=n|w&1f!219WkE@QcL zp#-_Gp15A*pO!E>mM-4RZKTWs3P|>42+?XVa~R^4e0*zFBD9-LoHJh?OY)@W?IbP! zSiew+5Gb(v@Fbr=!*Dw+o+O{ekS-*@CL(ndVnMQM!7 z+tvgpf?McScBS=XRA(DdZ1**aEBBk6Rap-NK=!^dTVWKCO@aFeL=YfIh^Qd#<$N_w z{2&`ei;j5hV6r+?0}8bVC%H!WJS?1YZDTOJ%lwvhARNTQOqfrn1cK?_vp1!E0F~R* z)o(DY1tL3Nj@ix;4OP)x+v}+xmRNL%o|iGA75@_pVBA|`d066kf#`j^I557l7@X@I zA>tF%x%B-|w$PkKQnK={PF(Ke3Og6};WSxdZl~tJx3Omf2Zla5w3(oomNmPC8kB=ock}F27L$bc#VR`&Vj`#4HNB7%G+5#1Wh)>=C-Rdl{{OilLD)?Or@JF%UM8MiAS(1HhEh=tKPl({eh*c zj2gRi@A-=K>5J_M1)AiF`(iH#A~{_)))4MBuI+y zwnYejB0J&{JR>rCN%|7f+b#v=z<-nh6KH`F;RD#9m<-~Zyn&?HFpjk_ih;`Eo*NWA zH~ZNfl-W_6(T?9uePDz$H9q)#n=u+2N+~suKnb1?^q@ALM8_J>C?4(Ph&M z{-4zI{Uh`Q5i=p*n^mv!-Tf?`DB2ChWNuKkXQVZA;{ivGg$#QupgRgey0PHa^kYAnS_F3~0)bY&0*qC$={skp@%D$ zV|n;N^=Ch(9Iu5~Fjf6-Q9!vn2^E!lL*IMSl?+I3TmE!zZ+s6oPT4<^;V*H21`+km zG)GFR+Xz+w;JZq7 zZ2Rc=6n<;hN!cxgldL?VLl1aX?y>V@S8_L1PO;+6#X>Wc4eXoa{48-|M3CPV~f@MzDLm+(y z_V==dBqy~Yu7w2xl*E5=T1r@=5-((!oSC33#{01<&(L7@XuxaXnYG62%NTTz%VTtc z!Z`$oG`%$jbE)2^susCDb7~Od2$Mu?e3DlX(E=rMZh|-4pcw5%r2;3DlI3#Vq@XaK?vG zSn;e*lTop=pFYD5g%~V`=@bnv_6i}sy#M?KMH;^fq}sOYz>{Adc*KmVz<1oHxmk!{ zBWRbW+{DYOHYZa+cU%LYH}4b}(fkkfA4C9wt!1p6_ozbbFaM;9InZ<$yg@tt<5>l` zKmy*v@i<<|&9f!>tahN01hYP*RK4SYC8lBoq8ltg3f7^L2Y(q7TqFr#{<{?QDdrg+#$CV_|s8_Bvn zEI?2>OB$vk7XdrRLLTdHPoU0LgV3k|c^*DydDtCZpn;YU>(GOl5JNl#K~YAcb}dn` z1$bNdPYBpcK9wq%$7k~YZqCVOo&09_R1u7f5EzafTcj;wfr_UW4!3)}NlASSl<*QP z@Nv6pPIdVB8Q|j=K9PwF1mpLIjlcRp%LR6i1Ma9aGS_^~4Z941Sxv)PwDR;>uu(iKSo#PV+>5W2#GGC-( zI`9>RCP^L9SmB*%wWJ)>1w@u z%E~rZpleZMH(~F7IFV5@IoPGam^yrU1Hu6CX3%P}C-*pZ`!o_t-aP1^J05@k0y-es z4s#9B{wtMH`+G2LAgGt_u*BHK&-;fXc{qsmPiMVJXP55P3(rSzADsuu$zb6nCcQIF zJpVBb&FqwDzPJAS2Z!&K<9chqlk!kig@6F0o@nc2ly5iI*zuN=-t{;<4vQFL z-+T2}jwXD3YGAmqL)1A``60$uA%5w(APjpxD?@iVOdiS+OuGyxS|Wf3f_OnsJXM&% z915N{LR{Hh3-Z<$9>vQ7*~;h=NLQ&^&=O$+6MBLo?3i%Wm2ptRslQCK9@G)3#M{O1 zqv4PHAZOkS4z+HHcetGJBhCSgg1~O!X7O4Qepnh2*RK+Ja{-ii++Wi&-fQU%8QXa1LDga@H9cBk>=7$pnr-n}g zfGF_uAUMafq9dnPu8ipW#{zNI@h^t~zPp14%v&Ri2ohF(seD9mi@3OWFi+{TG^N&Qn!RX_ zecEYPi~;!~$u)O-!5biIwzkW{R~;VeCmtW(E(s%NVB3QLqWizxtY85-3sREElC*Ql z6=W6;mnI%%f=`m!QRmVc5D~B`tlq)1PW?}W#-q+japXx7w(9s^^I#8OiIL=KKST9Ygk=L6OYLg_uey!0*Q zvj3%l0?mK(*;_9rFjn@7KGWHxJHU~s-R8hYcn!(!Qen*)~Zn*ZhXr~uOR zUFef|b_ULD#X`>54DHW)wc!naikfFmiegZ=roxnALe^WJ|4KV}+ky&C< z8>|(e17WnoaKpIWUk;!OFLV?5fguJ@1m^`FE;z|P2{&xWeHBlqkYW=fGo{8@vOFSH z@pbA8IH`9S05l=#WZI0>>3j(5{!Xxgog;_={HR|qe$xO14!|LVU2dJS{%=hb#0G;LdPZ|~ z!UtObE$iIL_X;9iVfFTO+3$bhkCZ)e{qu(%Bd*Ut%;XD8!$QnUh14*B(8Vw==3~nL zd+xXY=@jN@mg3CW9iXHJH;E0=?oK!!bNg!7=_A03s76+{?rVdgLs%1B6rQ)3?_{Jh0yq>pEe4F(E&iu#T~vWe1;vK z?ay%H?$;1S2jeudFm7tgOOOP+#{!xFsgp-u6D7hfm%ydbJX}8e!|voY#~}qN_g@Ie@(yMtlfAF?+{}rgz z2&9Ycd|@pTxO8zJA+@p{Yq)Yc7za$(IMQe>}x&}tw55Bbkl8_|9Mk%HTXPu4Y|Kv3RY($B0J~aVL z>bu%f^z6w488B1);JNT&4Hib&N{?o9+eXk<9ppVNOI z_?}XIb7a3e5+79QlPEN2jTF{o(?4`Bmy|Ad_|!aIwSO>T_%cT=ar^`O^+Vem)Zj{o zpFdR_Jv{axt!bh%wI7Ki^TnvtyffJHcrD*&%0&z6j25!(g;(T1y-EJ@#NaEn^bPmq zl8d?bGlJ>y0_m#Pd$1CS#ln8O=P$P>%W*BjAuTt}VnSY*$W}wyGL>WkFH2uN9ph!{W{4<-OxL0EvFVHL-=Kzsc=(TO3{lCY#M>^*$=)de9N8Ns_smBgriM>ji|y^?A}3?A_&PD=szPP~yMR z_<*Al@%AZff`gIIxSM+7pRSgDc3d8kiq(jsm5X3Ap|7r~TW*=9>xTtR}?4>0-CIMf9+k@i6V$O|cxkb6!H; zo-27cJm&BNN{mO14az}v)EoLfuaH*cSdadl5qsj~$l-gn`G9P)V%e<8#aSZhm^fmH z=JC6|Sxr1Dag?2%E9~dXO8QF$TP=2X#%t$JXpMe%t6Yq)avCW}@ZN#)ia5BqUvFn= z>~t`Rj?lYD3&?GB@ElePL`yb*Cwf2#P7f2gtzsMjo(1hQ1VqyY3qB6_`J9pjkI%# zB{{jt6EV~#@trx~+Yv+f>)Ce~oB6fx*(8E8yR|8^32qIri@i7w!p=-Ol#$ zqPDxlB;s@bnsmc?KauX-*vRl)zcCZ#59{1^;O3_!%7APOQ@@5crgmBb$nEZFmV3 zlk-w{&K4O$2YJ*|V}d0met(3^u7AN&MpJgAJorU6NAk2LowSwaypT z^;R3yWD}R{_J&PaC<>*LnT{Kr)>d+J)KVGWFNTBnmiNPVoQj}#tILB-g*i4$W-9!p zs=B5Jr@yen;!LJDn-2E!R;yi4Fj9ksgWY#$EGNDW+XTGo(rVOV_GM6_WZ_2E1{5dX z%9(p}O~2|hRnV)&^RIGP-V-RJ+PPbK$-(Jm zCbjB$GMJ_8$8#RfvD5Rr#B2F-yqoV$tMU`|?@YFXzGIzj7x*1dPYZAJc12lqvf_Lb z-)%E^G^sv&9~{hlfrmbfG5?t;DEjIwH}QVtGgXehN2WcA_=dkzv)?l{I*L9)J43vV zHSLJO#FgE6_IRt_|EuArFg|kJ{`n4{`)TzTqY$FuIx1{CgGwU5TYiM^`5J9r%M8}) zee_MqDBHYpAZIEgwz-%w3^a)+3xGHvio%r&H^`|CC11R_pfDbx5!0ie{MiT2(Y)ou z$>D0~Du%Rj65hpqjj@fMp>R-XbaB>c8Pph?-p$SzaSQbzG|$AVMp&-l^;z;o$FJ5^^v_io4kY~Jwv-0N_Cq# zrg`6*pO|k?$T@^}LKm}0U#wb>e&4Hp)Zr`G?5VJq`erlUFmvUx+0gTm{+;QSKf+$G z=1a2`PPb5o&-~5T9Hs)d3n$Am&1dVrtJRdgS!ou(@VIy-;JXrHNSL53szo;~GHqI9 zND;F&mnL{m8x?Vj^4xOdx12}9!HTSQ)e^bYp^6N?uXojIaewqUy)tKu@l0#gyUTCi zUvEzu2=~6GZ)gy*yXRUZ?Vc=$pyTSt-SZUJO=`Bup%|UJc7re=pUKeVEL!2t_j06o zsPMH9eqINw9(7X&c##XLC9R`->#yZRj}6L04)jcgmp zN45Q=>D;3NM0dFyY*7_c?dOiS%o`f)hmW2f@rryY^h+k7|MG~<;Q5s39Q)2zkgLL$ zxb9VqsEFIQmEM$RJ1&>5V)MT?@A%d#_=e|x38G_rHq4TY^1YU{s3sQliJNB;=vnKF z9nV(eb%>B@+<&D*D($vm+1P98u_0vYk(3q$IT<#}G&JyTlSm^_Ty1V_7^1GJo4+@q zLRP{2@=l{q(LL*uRksUC-zzgk*TOOBWaP;aM@nM@(o{M3YuP>&rKHWX(X~5p^ncx7 zC;a{=zR|`UYe=yJ57TzM`{QF-(KRZPlLtIP>nCn@D!)Hc46+ND6*29}2sjyua_!Q` zI3Zk8&ehD3)2q3AEivZ0d6EBURN82f=}WY@`uQZ^)3@|Zpn{)qHSzMdtVOp$(kl=4 z$=cTql1ph_D!USVyYe26ChC^>h-kF$ni~Ux&F1|jUgc@wGOCaGYCLBc_ihoe>BePf z3O{sU!i8Kzf_^n!xCQaW2?nlbwje3cc~kg4qaEe;U;1IukywnsXp1x@V)RSN(wz(o zwJ}*_;LLWW`3rMda0L~11(ylx@uGI!#`+qq(0YM@KIs@&)1Ht=!pzcazIeS3j`6;;dO7&|ICDrUbU36Dxxi>i5oc=Os+85;Irj$zJX zi?yLS_L3k?q%W2A9Gx~vC3GO1cV|yyptQDX<56)&)U)KK-+C3l*gH{t2AbT;TXRsz zv4j@wYx3%xQ?n#eKR5CA{z0mVa)exYPD^uAo%x~ zu$NpzQ*FDJ-*o*hP2#|v(}nKfH%_O6(zi*)ib|2-q@8W?LF_#S5m;%+X)aBttf^JU9Lc4S9lfFBW3-J;CQ6%8s|4*9p|!_Ak-BOSRvh!) zOSz!m9!~?Ln#NdrCmtIH8x6PyG47CL&8C+>8_^^c<(X}F@!D8cMtT@CR%@$w1EOK7 zcIV{tMr+!xclr{C&AQ~-yVb|_m?g6M(Xs@zcTGNJ3>G;nH*$+^FGDZxy+>u&{9H-% zR?Ll%QmFd+q1MGO8&1C^kDA&|SI+edD+FDyTgBrGseW!D%aDNz)S{*eji7mLY(M{p z9$T|my+rqI${^--pU6^9jm&O#8u$o0>qD1&NgaLgA(mw*VFua<6rA2`wq^3wi&O); zX7lX$Tys|gE_`xFb(MJ=V~;Jjrb-$U&*sm16g!?p_u2{zr+VwyE7Z;o>KnWM1QBe( zz1+qAm2wg&ofZ^Dab7wVk#SwKeQa^==7ygNn$A4U3lcpwtNuB0S_hlnP}esfey9x} zCqMr7{Pu8PbD;DW-#QU#ih*X;2D9ds;(eQxVG=Lg97toC*v4`GgDeHkWmD>dl48}? zhxYT*hig6Ts+t9FT$Jg$KciuXzLG0KLO?FfxVPA{s$NwqxIXRR^8jM$b)5ETrPCkD z7rW_3hlkJkcI%ru)^vG?JUQ>Zdn5yLlHzI)Hkn)rzF#md)0OQT_027NaoBiv7ffX% zL0w6dQVv~?XEdqtAm%>%FN3tdA_k&n`;JZf=yMl41s|y@pxnP0SGq*en}5%@bsRD? zUF6f8rn4-yYwOumzBSsxE`)q|ibeP}C|GukePqAn{HJ#5`Oidi%DV7rxh8FK`D}+# z)N>==o983B2INf<*=27}EN7apy(f+dSNz_6AsEFdYg`m>Qc{2j%CdDus%8~mQ`))Z z8%+^dQjo{Y*FixA0#~Kat0(igka`30sn7Qi?H_B^n>Vm%*NN}S=TRwIXPDlc(wIn@ z+2HaG@sZ9ZQ%hqqPk^dtlLN1xeOAZl&Vdu-8^wxE?c02@x=hRQWG24x7<-yDZq+Ou z6X8-FetX&A%F-**dAZ<@tDc>+?+>>tYw3Pc3-K4i$ZXMcl!(@u)udYg%~{i7X9+{* zFa6Ukp>a-%n=oz%ipLbn}-7}y+rkJ3X0f`TGUP>4wLJr}>M zctI{pYFYVx)PXfh;J0uzmC*X#>%z5sF3b&Tou4mayN@AvC++Bo93{n26^!rGLtXeC z-#dIjPQ(*I;XI{hn{^vvy9jo=Nx23-6cH95NvO8w^db9&_5IH?9lTw8#j6(=J7(vT zM}1Cqj?bkW*qL-1A=H$v-d5aTCt}o8UuVgG<49C1>Yc z`;q$XM7HU$qA2Lbws_ZEtbwg_qSR&+s0O@STcId2DbU`$?22L@qp7MZ1D3mtHfpCI zBWrqSlyAknkG=5sw?MdFNm#@Vnemy2IpcwFTeOG_wxGUNmfqR}E`_kQq zh-7f%b(zk=_lqO><%i^b-Bp(yHggvbVfy+-*{e9M% z{q#R1$3`|~Z&Q2`);VF~SL4nYU(Xczi)=H+z3k!o%>`Zk7*LUP!2h?PZ~@zk?Bt{^ zK`e(pU-58q5mS`0&05T{sRy2{*4ZY?F0pp=#04%`Azc!vNk!V~dE*fxCJe|s2wKbq-qx`|AM&i+6tDQ!*waxt1_DC9Y z18<+DrgUhRk9Lq)!zp{SF+QS$wJ)wQ@>;AlxJLXzNAKSQgjfx(=okdtL9I zd6n1si0@o&Yq?v~VAb(zaiKM2h3^m|(g5w9pTG zK5lXDy+d-bb8I#E14VFP%01SI!31*r{_^y^W=^=z6Js;mP)6y-bEsgT@WI{&rtsS0 zm_?*;{#TMBdTdft2sf)J7PhC43^s%tJEhj@-T>-zAO6$X#yhuQsI$*uJ35z9y+O{{r#YPUK6)&LRNvug_)0pIhIM~vF9*y_F&=ovE0ZO)9h^Ii+& z0$Ly}QYRy4TQr=Pph&I?dggH%k*wFD`;-*O=cn&W`)9yI0x9 zZZst{UA@U)CuAePOHU4$DtXs(&?ci`G@|&~6&Rd`G9@|`#sdfTOV$Gut(pe!Ut?Ss zMy#uKc^YnJm3g_6akR5&5L%LsS5?_+H?i4=wQDbaov9e_I2JXptF-6+vo*hb-Hzm< z`vHWE5hJ3R(jc@VEmG~cjsGk@-pT5>y6cz}&6Ewrqvv0LlqM*qMj?q5h`M#p&~EBU zvPKj8rP%jn+T%|JzF1H6>Z>=nd`({29jyrsn4fXvH(l)X5<~YsjrZ1KO!>%An4gFj z0ufM=6yPjV76(Y!4AOX|ZqK&Z`;aO`#I}%%McP@DMK$C4Qthtpm|p`YkNK&*$Jmv^T)4G%6KTc|JZ1f`eEr$jpm4bZ8hgjo8Gmj zRJSjR+-OQF*ByR#^ykBvSQFw#Jk3ewAL{M@+YPE2N?Mr4k|l7Y#xJJ5{vM*)HQZkK#^ovP{-+K0LB26li`EsA(XnU${tb7$+tR46g?)QNd@$!63O}ah_@?gvVtvoDI(_#EgGvQQ z`D?$&$hPD;bzJD7&8>ca!w*wY3=>7_I*6#6eiTs#zXsf;S(k`l^`)2@ zIA(Tu*`i;(*v?TFzwaIY9S?MJ<|;3#R+v}}IEi*~c~-ie%{_ZhW z&85AEDi237JVpFMP?)a%N2v6$w4ywX8c}0;Ai;gKlO!pW!JR0YkTnV0UjPrUhlHdHc{`S^`bf$=of)N)tpUW;iI)TLJUnhz`@8Y5$ zfw8fC=bciV`{`+#X81snm+#9ASrHGjpIi?wUd~Vy&v){e-}rX9Kkbg@ipt57GlXI3 zHy_fm+@n*B%Dd+B%LrBN6u{V8H!F z`eXmT%2T=Pf*(KKr08IBs4X+)L?=YR2qd9gEaD9;d8`s!>a0#_au0)MzedU~P zmh1(()g6siu6(u}nTdVV*yP$U(s`ag=h&mnlZz+zov9e8XPRSEmEss?8D1y*V#c7U z%l&!m5Ym?;xBip)M->bqj?ELrh*H}l!Q|q>U;uuNt^lfBhETagPmB;?9Da``&{0wrjQt%IC>B%EEdJ_6*(RUt#fEd*#=gV;Ty1X6ue zY9b+=i|4F(PXPP=gbu|7o*N^R6SMiMslBRJuJUfyxqF6mikG|RW*!V3`j4Tqm<}f- zGy|L+@FveaAh>^wF+m)`H){mqmzypf#jC!?N^A9&jRv}2cwM$`6(C| zDeTEaFW_1&y#yVO}awaX6pyNxJ_{P`X{LODXr?oHUSK{9DlY42!a z@uQ`TPsz>V?iR6OP2$gE4$aI;ZU;um-T%0Bpfj}I=KE;Z=eso9&K6lxeO_bwX2jkc zu3_oqchlN$r=5@d@~2az+sg<8?(LMo5}DGI%ZU~Q-^>M#xCb)fdPn&*wAQ7mH(d6A zWt)6R;ZbaJU_qo9KD)W)8OM|uFy(Iy&vKWcf56b>wR)%hN>6ej5yMtyK0wE9LV zCH3}48{H4|g=i}8+3G(A)cUFiWp(jdZdDpKH$u;juN|McD=2ti$FOu#W%BO`eyni) zUJ^a2)vs+}tuHU@9Vy%2@9g5Mo1p?Jro{b%{_U-_f5f>xan{h#FwYk)^RngbX0UP1 zjxAM)cgFk&g|HG%6!?sNnEzYD&9(JQ-T^-SDtXTMq9ZzQDy}$RU&~k}y7VZtTo8O! z)&9<3|4}S0M4>ueJ0s^{PN{X{^{}$UmsQ`}GK2Hv@x(f;e#rXiGmzxXz|HvN}68Kg+Yel)6wOm=izKk8bvPziUUx_rj0& z-mtUm*YMa}sXA}6#+Y=#`{UTPxII|BD*mu>jR6E2R{YW(83-H^Og!vOgU)459H46rO_3#t zDyI}KpDf>LT(OMd=2L;9JkJC9B5pn3L~UJT^7@hd1f$UoiNU0mhOj*{+NxD10-A-& z;^dULHy@o7bXT_r?US!HIPI(IICiVA9-ffVx1oCD+k&I|nNz(N{qZjL8`>}E9Tdjl z#YzhtHla+Uc3c9#d3KVZT-~FpR`bU##d42ONU=Ta7A7WqXgUeDQ*fRmk*}!B~+`ZN!B&5MkG*yUu~)1FZgvSnO5AXP)@Exf)eYO~;lf0pP$GhMk%nCuF-DeXZY~d80pzf#CXRn*>TfOOP#Xa{fU`QPfV+p zru8l)_B&J+oU2x7T3&0~ySZ_iP^E{%r@y`IjAG%;CQzhg*Fzzs$o+6fI(?{o`yZpk zx8z5*BJMA5-A>J1nc5lC{CLA*x8Vy!^?)|{J}a>wn_c9Y)PX?LLqrF!;Pd*2znbRN z^6w0M@wI&E$7|T{T%|y^TB`(qW#QSfUGQCzhdXC`B_?R%wUIX13qO_nMTi4wGzp&@#Up*nj%XS5avHE0HTBIRwU{LP6TuNE3PS~mNqalv8guackt6>U&ar1>hkMbUi z*rJI8;_jdGKG~DqVDjkfKN<324uY@ROcUcXqZ&@OX;0OU6g=RxpU~>c;5>upk z_qJBr;`{^k>5QLB&s44ym<5q)4R+^Gpr(Ls3{6RfU55o~N=kqR5R=Bq4ei%Hisjkl zKYm!`e>+L#s9sQ;lU(IrJIZ~q!_ShLQV81-h$_-C)oq-p+6dsD;eQItbWop(7j%0a zMr-MLWZk!VnSGpGzX55T3pUK$C2<9{rs+b4yGY}`m0gv`ypz1(P_9`+ze76}Jw{LWJ zE$E;rvkOGnGdvvB8*)N~ER6t>+c%|>5iS>k9-Yapo{Qf20QG_yz^{cuB5n(;t-mX4 z&B^&nuvP#S2pkjApR`zvB-~QtC@k1(ls`KVi8lsv;FUJDA)&RK`%{3^%Qi1sn-Li^17KLd)C{-cuJTOigerq zYcKw0T*LzwZL~lKmGdx#fP*ApAh$_d0PRJEy;?*JXOQ?Ic&zA5Yn{b;y4IgvR`%t@>-geN6m**5 z;%2~d24xvlMRo!>mfW31l^+aTBF^h0DtdOmhsxZ*#A{gw(rnDC1@%P{b z3VEj|+|d$n74XKny`e}`0uGsV9VwK~WP*vtw;werG z0?uOV#d{)>#K9I3_K0eFggc&Co;pDjexc5JO^v(H7;c{k9$-UI)(A#UlWJ$04Ug7pUk1p-!GIhFlhJat+W3K1Bs7O z+Rn6lCC1U>@0WjfZ2xRM3BmL@ck@F&v>|6#E^ce|QKTb&8{>ZvUc{PUOK=fUm$o3x Q1wX$SnHv@zJbwQF0MTS21ONa4 literal 0 HcmV?d00001 diff --git a/Documentation/px4_hil/SITL_Diagram_QGC.png b/Documentation/px4_hil/SITL_Diagram_QGC.png new file mode 100644 index 0000000000000000000000000000000000000000..ab967ada62bf3278f320684df49449de2cf5573a GIT binary patch literal 75957 zcmeFZWn5HU+dixqlsYt*#L!(z$I#s=9ioJQbT>*2f^>I-w35;iN_Tg6cf-5px}Uh7 ze|>yE-5=n`?AdGWwa#_+acsUQDM&s?BS3rb;K6fgDKV7?4<13lk1onnaOLHnsIUhQ zNFPXx39Gs2@60}lR~x-=KV+$Kh?AwD3vpl|l?rr#M+f<(--+*}eP#;sL81NZz(7S# zFXijt>@0nLm$u)`*RwX0kdl%z={diC$uDTs-(bJDojiJ-fXyW=B_$PTL+bMY3FW_j zF+aw_fNoIy_XQNvw~{^wZzaF5{_`5CA2RR5e+)yJz#@fByvxM+=QS|Vqkj&1D+?wH z{vz_vYeFvRkpFr(_`-+jVT&&lH5%DJSGD*_;GZ9nsve_2s^V$={<&~J9Hn#=Z`ZAWMeW!7 zBYWu>>X2syg3?2#T|_0Wr3MZ3)(#(-@cUC^*7)_KO?jO{%g}E)&P^{DqndM{Et386 z*7+s?1SRfsuz)MU{22(XEl4Q&86<~ZBjPU2>*b4+y9W28@p=QJp#d@zzYMP=r=hDH zfs8KCOYd9b;VxX2Oc5&A)8Nt;z4s>zkd^%>$Js)Mkw)RS5}nbX|s|0GQ@@YGB7sLEe9P0}b1XVKrb?jt-|7V9J;BFd@Y(93^+uhAR$ zlmc?bM@@w(^s5eO>`|R_srkKm-~KikY{EH<5^=27NQbPjWF9YFeall)oM!}F%60;Y zf@*b|76hLoY2qO5ca&Q8E72AjB^|$BSgtJ9ln?EORA;;SN+?&Y4!JgUoRdHz2|@^D zXYq=S-nLiyA$xD%jv#+>p%iinlnm)DCI`Crnihkl)wjfZLq9Lq(9?b8FFK_n7w)A) zQGvL5Q-wIB-?AM^trA`FVaT0F{>M2VLuj)5)5EepMxvYc#XDkls^+qyXZNQGh`R(f zEA-@m-BkMELXTn@mutV{%M#7hH|vH(AhTrJ(IIRJ@5z#}3%AqTCfE4!v1w0P;auDu%hRy=E)NpM|s|Lb(b0U(!1^km*%+m2fg!z z4#&Rxk#{_Z(q{j#MHIIGUpX!mzU5#o`?C23H+*Mt36a{3aL=K)d)}b)mx|@ z5g&WQ)uo0?=DQr)P2%Ag0s#s=e<9gYp2^Lej@#{(XfVaMa`O8efvmyQ(vK-bn)?I=@|vmV6s+^p z1=-)0HGiWYSu+jp<2#LN-^DtW;E9mF>W+nTWLa|`onIux1aaKU4F>8=R>tjv?vgICKb?3JQ5lT zgj2ks5pqEp#@(B5Vo9r?k#(}VkTmDAWG23DTBCh!-QtqKY4_8pGgb8IEw}IqKgL*w zPSY0Ncc-xa+@4qaVGD>u^dJL^_h(J}gm4$0Kvm*7tB^hmOn85LNT$Uq`Q`e2uk&7i z$10bFfAHrQ*!k209ezJE*?$%FuW7+zgmsxi5Q8az7f^jqGSwhE=iOMivC*nSAt8Qo z^b_Gl`Skb_cXf6@&KAVcz}m9rX{~S{3gVK&F3eh$m=Ie33pnn3j)~$VuGJJ~;?@~r`Mvi34`?#eN?kTMMRw^4_jKj zH{J{0O5NQUhpu#-f13>DkRHm8;DjeMo``Vqe7A?IeS0IR?{?oxn-qG0fzF>4CidWy z5(rSdi0I+t!}WMc&E?=J$~ZzYOPRoXGgr}{Bf1wC`oOEY+Y4ZVCmqxPU0q-CAO&ME zXn>~KmTA@eZMzi@7C%7|k|bSt4y3__Q}Yz@_d0ljpfza$A_P7T1hWbe1eqf|%OEBx zOiVJ&{%><)eAx2H^C4m<$cP{~&@n@f82kuK1PQ-HK}e|zJnY9Tk_W<+F}Q%pY_o`K z&=KFrfWgF%!C;|Zgy3n`w00nx3jOK_1H9&h)F9v+6bv9OA9kEkL}<#S04Em;T}P|} zMH7s;LPE?5LB!AWh#GOkV6Y5i64GN#gj#<@|A_`!JzBS`3)(&-wsq}m?j@8vSr)RsdF$#qF3oSzS zkE4M;q$dVoiPCE{D3gh`ny0a@{zj7@ zNfmRRcS}ERJsZocs2-j88%?S`L$IbuP8S7|*Y`2%>q}@1<(H_sdV|7WgV@`YrZzm& zt8{?~Q3U}}DQO3}{cSoJHZ(9Jg(#V)m6o)5%z6>dxjk{Lmv$Z3#e|4%=}f(+q{?He zn6z=Ln8Bt}2dyk22!sHM(=v(POa)nX)sAHf4TG>+KH@#im~7udnM#4CMV!RHHX;o+ zB2_G|hu8?@3E0RL<`*pLXht*D&d%VtUK(?sZ+e;Gl+w{sRZt!?C*4}-`^N66$968? zKl?gHIU)Js5*68_U@+IA0LlLC+8&O*Rl`SUy4bJTgN060F$V=wI6SAYF}zztA)vPS z8LOKn#Lb9B>+7=64r0D@3-8dqjdiRD@0S<3%SMZZdtvpyPRDK_m2$6j7NeHV0 zGnpgX^EkkEZJwm0MKr;KXSJtEqsOV}%9O7?;E|*r9{S3G&HCPRtr{!8;s=;oHs4fA z@)C~P{&w}az!~V8N$)*+__&@PW4@)e{!@Q<|8`Z^#%~7k`;Y4%xG(tPek1X~HhL#f zF&-nw&H5nh5&n2d+yzA@f|c^8PzLw$^LIpB-ZN-OUNknhT`}Rq$Y<1n){7asce*B@ zP6qAq5i@F*`y+6S;QrfUdZnWIk#9{w+%v$AgS?9KJXfWnmePy<%vlR7j zGy+&|_@)k!Id3UYu_(g}aJ#rmDm{9i9gN4+r7e#FruFz6$tDu?rtFS@tH=t!-Kvc;Ukj_OLMvw zD8jZg>8G!;e;iym8=EYcF1Gj9Oz+h8Si|BB8X2r%Eq0-R%twJe*5O;nHBj!_s zolE!aOlG&ee?t7%71Qkz2O>i%dz>CgN5GNSZF9L+Nm}V`8*495<$EQW%$c# z#c(#1AIj&l?DjHpKT=IP>xJE8l(3{gkm`{NA%r)iq=pTIN3pPdV6(i&l)&%(g^1^F z!Pw}@(Q2g0^KEXcv*wOgx-e_~*1$RnNbU4ciu?KMT*J3{(uUIx)VR|i1`8Grm!S7%TB>e9>?xd(!S|<_W4yqFPrBGyU%%oNxy-757_{ z*_W;dYw&je1B_93qDUG=>uYhlIfqs4zrz#o;qPQq`^6tTY{EG(cG0Z1tz*1&ugsQC zkx8{UE}~S#!+B4=7l1rlR;iLvufpxF-Wg4uBd1$6SMSN|9#CO|KF+{pHCbQ*^puaq zr~H_lM@JJ!1mV_%fH!y$B8Cg{c4wrkw6$N^SuJGVChAyhR}xFg)H=?4 z&#UWH^DGI+7ftu(bo&LGS%TSxw_dx$ndTh^a7^}kE9 z0J!S)Ii*w*^&6=#?Pydso)Q;WV>&6tX*-kNnq$^e%z6DOaXQsjNljx50~Ll$$1k25 z5bb@1!S2;?UCUgAUpCq#6iqveiE`acCr zP^=V1z&AWXPt{Gg3Hq$R^hMJ7;;^VF1>ts*Wa*R#6gT}VCppqDyi#E|5q0 z%ffK`DCnj#mr^EVHCMA8`Q`tV(X>_g40rfOyA-8Ml9)D(PV#EaV--ur%Dwa?_$!%V zU%_SI*r7vw{uq&cb;k4N)I?kL)*k*+)@oP}_nU|Ym9}5zUkJxf3KFpgX0+agCdF9( z*#@nNWt#)Gt`5#V_hZ`$eaahi|;vOgDq7{>!aT?PmVh( zb*hcK7m>55dHwSpN870!k4P`w+r7%ur}~6lzGorghY+a^@K2wH)xF*fceIEuiJ?+` zqQpocAx@zX2!T<`#1GUrpU>T1n|Sd^?x0~n+Zbc_<~L9;y*lX=8cc$Xq+&!Q8YU#H z<{By5S#B7@R(q)D88zvfET>k>e*CX_TDLbn;V|UYELb^q>?@%BJfVI&uP-*f1LdSa&ON3!6uidLEo+FSqh9H zC6$&%yv#HzJ^Rb*ihnn&d#{OBnPXV?Ga33_i|DAJC&yA((R3?Ar-Vo2RSieOLyl`Y z7VDzUvSun7*>u-ZSL~7Ow&XQ#$5lH#wh~_Yb-#<=d=Cwq;)q{o5p67bxf=B%go;-B-Q|Zh>oW6xvM?RnkEKqLJo5CeRuf{KNmN-CMJn2MrhRWE z9=u5AH60@Zd8&k+v5fT&Nq2<23Ke=JHvT=wFlX$?N#eco@*G);UWS-REZ5Ies#|I% zYKu#i+hwZ*s0p?2e0jz%13yefHa1sV`zDsT99z9S+@2q*HGi3}!kuQMkR-D=TW39` zJZ-UlIBYfR-n_tg^zjl)g?*eNl2e@Z^889(tNQ%A+?9KSuFa9PT2=gpQ_9r7#BG%b z?+7CcF4_v`?OwB5>@w?JX{E`w!uore`@TPU+XvE-G_p_4)bpcPI+KO5sAN7|9kqn; z8Qlp^&lZ%IqGLnbKK5P>7R?*AetI8pAKWLuGE}*J)O;eome#%6PgM$QmS}MQPcDF} zthP7LnDH?NR2NGbI7kc>T9CJpuxpg&70NaYiTAvl)c%d$)isN}3iA84#qE3*@tzVI zl0Ho%Xzi(?b#JOD*o~dNLTz-X@z_@HQ7Z}LTxT4<#lpuGGm|i!1h@BmJI;jb%a$D@ z(yRrAJ-a!A-_%%^ULLFFpvk6PajXm^pbuH}oS=vYI9nS+3*bGs!!V^uLkJ6V`g?Yxm`ySaXobYr;&^7!}On#M7 zFKC%Di9F9cvQZuyLM5)G!sEDE=GrmtX>lYNDgAjkk7In;+9E8qZiAa9^FS%Z*3XLLlXC!58lk*<_>{9NBl8-7GLeLt|^VAb4+pHYL5Bm=(?qh`}LFS}a=Tpx+ zV|W_!$4_;tS4Tm-?eIP=&?yK!DKXHyoAe&hoUeD+;3VcY`FPT-JpZc5v+#amGNWbS zrvNeK`vRO?7A}|iZ;Z2z{S}s1YINCF`7P9%J=dmnUv(>H6k^`zfEKHR%K&H6NNJSE z`81m3!LY!ATNLlmP5HiF73Tk_qb`K)8#Jp6TnB~*eVf_5qd43leCDS^!=o#kLtzOV zkGre(*Dnm+S2K*`K zZGuBmL|ldT=`6+K`{ElyK8rgC%&Nq^=BWMj>|!=-62_y zC;1XufY(R!C~@qyr--DwKtd(=$XNV-bpcD5V4|u?@#M#CaXo_Y6R#6;YsCa>q#VaI zJ@Yq_)VMMoi{lTeq2M2GbhUTJ7joMn8azsOphU)F;Hd#Mr zCfC{Z`Z#)JenyQ@_x@5qh6?eT?9ImkA#LFYXyd%+-B0-jIlfXVg=l+v$G_Za857OE z%TXx)0kt|!JN@L)=d-tVKkdyw_ZW%ap-s5yp`iC_t!qNmaR$Vy++g3N&-MkHHK%*m z&Sr^fx`TPhaRvnxk#Klh&eNpb*HezP=A;yB7hez4G0!%o5WP?!i98i(XVGKepD|fiwp*GH|@UF#K zl~{1NsXv-GXGg$`dgde>3%GZiItjbOEyvAQCp7Imm?5wYoE)yiG1r6iv);$;qDhXC z0rJK3xxVcy#hu4`bLQt$F*&xEOGnqSvqz)#O(HQ6nPkm?^AB`fR`ap<^4Fs$uh$PS zl+%0Cn94mybMjG9iyMh7KIcHyT{NrSr+M(M3me!9%voMmxNeLHg})7^wys`h54j&XW(?qdJyhtVK|2B;DCnI=Q!vS zOD)bicuM28F%@-RY4tXjkmJ_jYt{#~MY1xdQfI}f#HgHP(1wjyL{eQ~|zFN(hBL3XbW_O{}$y6jEdrUFFBG=Ml?TVhf z@s-bbwSPFx`{p~TyV$%)>{~AgF8MU+yp^0ly%vVN*j!gE;&M#8;31XZv z4b;|!F@G3)kR>buNtrIHt6i^ibisl;U>29WL2(xY#gV6fN$`N^{(3_`dJnn2 zHL<$OK(!IyzEIA1b&KyF2D``Ne1-sP*}>ag#++dnhvHu)@AFo>dXq9stq1(uYkea? z;$8-Kc5d|z#rh!RVR=I|>DqJpLSFrP3%%v3LNXy)b){-d$>l&LN#Ek)X_^-*m$8Dk z)h_vJePfG`RMA1oQHdAdl(>$ccW1#=mXDn!t`elUT~ z$S&T2oy&m#P;GI6z^|=(H@$-mkBzYs!PDIjPS|LK(35?KLiyVOU3`>nhi@!t76j(_ zPy;x!=Yw%%+XfN~)p;#POuXW;d>u(D!*O_kZq*_Ogl{(SBe;*+)M7 z9Ngthajq+)&)bEDqovzgNEt5|T|T zzZwI~Z?higf0vAwU#v{{fPjKwgFHZiip&hI$hl`D$EJ8SrZe4Bx*{3u zpVFmnW2(+p8H7Z2k6R}Sy0gJHn3Yat1gX#W=edT*ibUt#_>V5tP2!SE*Y8(dO#MTN zHU#bnXdM~!^rH7+N9h?t)z_mb5e{l1klpvbQ=F0HJY|Xa&$rN#i(Ank+D-#K;u&Lk z8^ODCR?6eL_rLdZr@U(P9Hz#jfCN&%iF>`EfXtn)nEwmC0PUf=C-q?3>fzI-IIqo< zc#yhuflM)5$wnRfu|xA=)Ni!JYME`XyTWrKfY+XW8s|MBQzhX%6=o|tM`;75&#Rq9 zlXu_nPMq46svfB>i%n_wclE`J+a+n)XO?#dkB<0}ncq zV{=wCRPj!l&5`!a5owLxZSJJ8dDoG*NXTQ&-MYnJuh~AQ^C`Yzj@Dq!k){I(!E0d= z=RP|xvkZHe0Xkb=k-;)Mv(9@dJn2a83|C0!jdzvg#T~Nq6k~2TXecnq{jZ_n*E+=l z94W$IRO3yh1k^z$ftd)>=6kR(s}5xJKONJ;G3J3NKoI91^L#E3sIZ=Ub)$24^Sxn5 zZF4zeKIxsSZ0rx!84Cd1=x42n0Hyi|@#KyQbd+23Z{4x7<)i39-_;+62H`0;jhW&3 zd|RLlQqEB#H3S}pq6u2vzc`6;x#lxXdTBYJ-TRz>8ly~GciC6Z#Fq|G5BXuq0^i58 z2HrY;DZrJH9YupQEgFDkX3=!Ld{+!TJ!rPZf_CV2{W%8IL!l2D0fpGAfH+8##Bnz> zg8dsU{z>~DRJLj-0#RU`SGcGOG{WKc5>P}tnBV+3d9vJ}z%{i->!+eufAMSA*NT)|NL=-m7`%ra46x4aV5582rr zY4vZklN|Kk23QcYlYE77Q|LS-mrqn1Dz|4yyTFEewDiCcJwIE3x?-#+#S;R^()sZ_ zx0U9C?!X_at4A?~Llwf7X+_?ou%E-|vK#&?E=!U$wNA%52}k-Oz)D*Tgh` zL!;cFvAppD;X@d|!ZsweAt#Tn96}B`LF177H3?WEN*g*j)A|o4R#<@if_7oP^67xI z%yY}xDaqdsjC+9OMGg@5sDN|=koS@x8pvaroA|udUk|Gl*p3!RZiBTEcYR_8?adRs z>^pGR8$=uU3)_14;BubFQ6qATuSOjzKH^Z8ih(iDaq@G8K0v}mZ96@{&?(-lFKI<% zmD33-9$WYuNk)LasnfQgqYeOaFw<{Yjm`vn{A{Pwphyn;KYxu@Kquaxh-Uue299Cn zQWud{)&Z>dUnvHP4PwlLyrZ92UtunC?Oza^FG2?nv;p-SLby?232QpO2N=hHJpKla zRgn(KVQo0jad7~EFoaWZTS#56J;!dF<@?&eM*(X~w4%i(#2Uid{dL;zz#XX@vg9Y|M_Rzp;bW8Cg;OCe> z-$GEEegf))O@HtZ!V<^9k=7edS)_bHN2D48K(#JV62lPiSqo_Rp+Iib{}wUm)}d73 zWi|xDiDbctPXQVzZ9tmuj8B=-L;6@WPasi^!*Dj0dKt&8tzM5z0<<4Fey{IAG;gm%zZQ2Gdl8eLoex;;lm zJmH!@g#dB?9?)=Rc*h-V6|r(~v-`0?(1>86_A>GskA z_#hyg^kBr)3JA+-^b7L^oD2aY3)-OI^VRN>K2tIup=Su?{42hj0vr}tT>Z5EYj#Vz z5&@(3(d#3tyd{tVU=#w0X|Arff=>TDIJP-U%obmrR6peEpR_*FWIA62bd*I6%u89{S}*xvfl0c=7jVLWE(N~UW(I>l(Jpi!{8BH5 zT8|QFkd!TNc|KWcLi@%BG#*%fy2Za%56sI6IhUzsRocZSIJgBY3Gg)F6H4oBU`f{C z*yk(>{$EQnT&w?GcK!>~Y%0A9$AjJZYhv3e2gG@QE+#@$FxEB;F5~-$X|*8?UUTB{ z!(LUZ`pWtxk5y=NvC{A8CdchuHM#`0A$yIF0`$Vdogd@MnNRwsVvcslNjgh*lc==H zR#bkN%WrZmJnD?63qCf*gz8oiAmFKgP);slfN8V>G|}iXG33alvpriOMCSDtMqF#_ zk1Y5~u|_JWQNz0Zk4l{x``yJ7thU-H32 zYoNRJ866y8ZG9*gtgsC$+4nU45$t9v(*1qK??zrR!{K|oX%Uptksa`~u;`~8veL6p zy44e;;S41tMU+?g2Y=PYefJO#kkS@^irLRCbzL~eL z%&PuY?l*bdsE1Le4al9MP2bw{qJt0>AnVPp^*Z8Wf>g0w^_M+xMVOu z-EbAv<|v|GrB{!t79e^zM(+0Gk)z9LCwV1b;Z%WIuYS(5+b^~RoNpYenter ztD0uwS}JTlYV9CnU5(3OJt@CWcrryscAhh`jGJ>$NFvqT^1B`nde`8jXMX0c8k)+3 zZ@Au(KoxbvVHBP%pU~3KCu)WWg@FAb+E+?JPoqE*gLU+GHiz2VVI z92vBtXz}eB=%0_PDbMZb@h_y5pK`zqfb+0XcfhSrqdYFrrISMWDP^nPPS$vq->{p+ zQ$ClW9>y;Uci3bNoV81Xc&X-oB{441C^clG(=R9@;`dOJcz!)ndviki8#Ur94muYvhQl6K+kZIqWwPhzeTT+YyGG3w=UnDwpEvR(ZusfTn172#PoQ@eG> zw|xy&{Sck-?ZtJ4^^7L58H3MkrqMeIY%(?NXtk}Im1Ta&rOI|>AS$9G{dfsw6x2hu z1*vLzc10%H4`C`LA{>|R+7#E^-h16~7PO zH<=y>WQfciY}y;QeRb9C&wb!^f}f=K8|OH?38qyN?cj3NsYEuP(tnc8xY&%%LAVZk zQ}F#H-$0a;h?{<2PV4RRvw7Uj@dMQSH<{@-6%HwKdWQ>yLND&IzP-r|Ke5nM|I< zz}^AgO?h69ex;HHdW&Zf zbD)0Ra>Y!Yv!VmT_$wCcs5<98sTdWWoGEO_)nmhD5NsqE142{DV+#3i{un$8Y5ZCu zQJlBBT>2;e>f=Nui_z#L5VoV=@`oi8+k+Y6swd?rXa59Zug!T7EstOW-?p|hBLtO!b|(R*q?p_at-SljFdmz zXR{jbU_v10`BuL9-8<6{b=3>--RekgGrX%%u_ZN1m#3P%bvijDJ*r$e+gd?Kk8C?6&9Q;FLdMLCK7>C}VV z>ER78E?!ky98G`L3VnLJ(J-F<;S1|gk2j0U@B^h>-BlDqSSAMq{$eMlk7-+`42Le; zc)BF=wLRWsjb(XHDyP2PdN0dzE)(l;(oXC!B$dyitX}YYm|C&2r1@ArOP&Hz+!sMF zhRq?<@J>ON7`1#N^rbA&d9BVNV1Y!xH|>{nY2slujm|N6Zc?4|;dVMI3iCklA+i5o z+)I*dSQ!);zHUyTgECmqchTd5EPK5N0{|m7GicEL8UUyKg4H?ee!kMYw{f@}xCy_G zZ}cLqR`GZaQ8jQ?z_Xm86TJDst9Fdrm-cZmU1G59wnK^|xng+m zkJhoA#qwfu&uIs-MdAMWCg-@X(7S|>vlaS%X?zq3Wkyjn=3L>>27TZ^hIj>9ta$hh z2XC}ktxmY?Z0@1b39MjW$~tZhM^#$!yAEOuunor(en>+jl!~ExA5C1j=7ZtOv1;Uf zSCkcR9%bjpnHU2P1~(g6dQjRXoUht92vAyl3H4q}{&5(}R57EAy~gYer}x*Aa{-vo zB;eyGl-Vz}G=&z5gS1ztc7!$8R0rUzK@Nnkr4!W|=@era8MTITV+pJGJJv+ojYgJY z>^7&K)1?Vkvlpt#9GP^If*>E=K-1SXA*c>dTuCI z0X7v$<|wmyaO$$W)H&ERh`pBDn0%eL!{@5b%de~-WYkYBEREgWi5_QM)welUE2;97 zD&~x%OZxX`S4z(=9J|e{=a&}K`sJLII~&SJ+0xn4{*k#w+8bOI8rE~M{4{+X{T9{n zm1tv!%5@SdFz4@p38qiY0ugi+>8>nL`P0KO@3HZ-p2ti-n*xGa>}u24R18;?s@pQo zQJW)*WEQsrOpDmfw0|)qJb%Ws7951L0I6k*zDJdb)D%d(Oe&hPuh!7T`cE!E2cG+B z`%&~7i~6i}yTio??4!kk>b?0$jHFbcRK#I=TT%{~)CtvIN%fkg ziI?}FH7xUErdcKbE%f-+=} zz;8ExJ-w|MW^y)l%gFyAX~_F@Mp5~KXdIY74`;(mfbV&i&PQ{&zmT57>FnikyDwL_ zesOA9boL4GGjN}qPVHU&DmlkUCM(84>w70~6Krtz%5rB0|3yj-X~Q46umdO{Wsudd znTC_u-?KwH_KFa=oy* zz2YjEuEJ87XqW|_9i}?OUl2i0&UL)|Q^MFgqdEayyr;!;UFqX^?7>@&uIOWJ7n^NW zI#40oiU{GWuxQmuo5!#iSJ{Q+`yRYiRbRhVXO14N+KtW`nEKKwy02abTQ;BKn(B0F6h``ycx!Rb`J8 zjy3qZk`l4Mk0^4wI@4}i9Jj7IUDR{7yEN=v`61_&nRVY!NHSvTC0uxpi&D`z*{HngeKVD0s$ zR%zl(;h}t@DgCj*gTu@tk=X+3h0PCN1W-3_6(_+G*YuM20~R#0k(%Hofs~i0uHI83TGji3`Q~Edz{}@*j2&vJrjgw1 zzHf<55q*^RRFT;1qUlhu`;uY2$BdWvSidk&UUNjC&{G3qu_AlFGm}<)W!}^o%Pjvb zP3v@&wXexk{Z`*vMW+DQRZV?YfY#UK&LnRtB9;tvpE5^3WjeDilDT;$gBN}T0B>(X z><*CM*t#$VtF5nol@2ra>DDf3e)wy!!}s)gs@Dqgu=n#*j6!d&Oi9_!7`g_=xq6+( z)O(F@iIs)w>Qid&GjenT`A^(j06J7%!~0w6+{vKvScn^!*l`dpdlt5ZXLk}VF5(_2H6Z|;TK+yI!@ zZoiqeZ>3Hgct<={V*k{E>r+X?a0Oj)S}0a>lilo&%(BM^&BR+bF8fe>rSG|0Vio)) zp)M8qS3hqmJ<~kyjo~boV~Uiq*N@y90Qqs~t4BSXapPqKNM#7#tY--N>vRuQh)8d$6lU&lJ9YVC zu3v4XQJ}r$WxaS4muu3NH5&I`9^?2VuzykbdOb)l!~34&X6r+=0^4we^1A5s=xB3? zFGGfsAPwy2Z*fE5p%E__$vb*JaLd$u$TqrGSmT}O_&}K$ya^Du7`4*l&6lV-{sTH7 zd2?0GIDz3d_k&~Iee=xoXwn-JKlcn`xG&!HV=SMKfcj)ZIWH&yigiY~+0oJpxHSoz zteEn?1KXRYc}-)Zaxd+mz294WOKvZI%jz^~L&gEPfXY#peo7m`r}?zn!bk#*oz#y% z_hg?jKk|umScIxbv6(G%p5XXB)rGaQljp!t`ow+vq{(wXC@*i8IUUk%g;z zC1@Jtd}Z?a{RmmXaq+>zOk7I0#lpb+la$=?rDGD2QH^mi2coM5f9*ba0LODnf;m8^ zicL6M+00m69J%919m3rA)#m&I_r$4s z;p@LZw6sc(KL6JLq^nn15wvAJfaH5Q<QYGSzsMe1YTF(-{kR{`s>buhu0ot9MAzLc zFkBYv^4F&Aw_R=qlZWcx(a|MS6efh?3@+Y~b1|_Uu7*aE)8?OCI0e1eZ}fU!IzO}B z;_i7-CGi&a()GXS9u#POxCUfX?vu*LaMg<>Z+A z!&$_2{x=MZNXK4c{rwE@_#2mjC)ayW!#o=a2d`)T$j*f_t_Y@sK1$D>Ma~Uq7H?#3 zQgcwu7j>Kpa@@$2pH(&iAR-Y&v81IHfv%p1pS^b#y~H?}lBsRg7}6XEiwe?4b)a{%Ns_EI$&lhR4@c^0kJ3m3%}Q6tE* zvOlt(LB_2%q#^A*m)QoO7tBFp5qJ&fe@McrM%C0^0zaP9S!l2}XeN;fCi)-6!kJe{ zxNWWc(%7mr_y(WM_uY1?`?uiO5x9n9h2fs8U`4XIS1AEP1|Cb4B_9c6Is~--o||jL zeGXeY+n7pCU7?4$xan!EgXS*kA;1xYez_x!h05|LaYci-U5@7bQ%b~K4#MBybI0{0 zi5B4WF@QJzIPGT~>(pndq#&SUK!DP?iRZ;I9$01WS-m=Jq8%yFwtbdtIG_JkUa<@i zIEsgB%T?6)Dm0B9m~R3eLG*v@?Jo-BhiosIrsU#Gj?gpMfM#ATMR*B=jlTQgt*j-L z#ngxieN^ZJO?WAQs8d6)K-SYz#5ORS>mW%>j0z~8fnzC|C*LJdbyG}Fn>HD^x_-W* z5OymmxgI+GX z_;qe?3S@TbG_R4kS3C6kqVTxB`4gugGC~K28EY*D|WHC~R;w>b=8w zoNmRmHRgzCt(a(Pfu^!i$uLZ;^A}%|CI#VGY&|gDd{eQ z=5bWcV`=fdlhkjj3EdGMOT@xHqp|}$Ma@NF=juT{t7M4h;so51AiinHI_Q+}{k9Eb z{GBJTeW$|Z{W_S|VEtpoYVpyOoEB(paW3=FD6#X2zs(pXE$~C4QHm09_g`a7?vC5~ z{3nD*-|L1Q3iUec+4rZ#yL-DICcR-an7T=lI9Z&MF3zu%#ES=6JYT-;X7PGu*uCXV@WLTQasvNgbH?4%rBXjW58q*~;EiwW zX)5k^Ymud+V`9o)6%OY}&2i~+Jb|lCi7zhcP_{G`KVP+7o*Oi-5*s>l*t_P~dm{ck z7Fi)BDhag4wpKc`r9z8L(Df{<@8xwqaE5)i$AIq8DkB&I2cZB^Py}|MdLvruwDxHw zrSrIzUsr=EQt>H!{`*|F55N)QHKIUx-^k!oZ&zQW_1x**&0X50_<4zz?5hlzAv?AB z#xf3k!0tsvY!tEkN>XdD*uE@3$vLN?g`uFtq$AJ%jyN5b0Z*_cDA5G^H;+$c4+ z<9QML?$$|vo+jPeb6!__HNxx3_Zny8%I^E zZ%37IlLbPI`r{Q+hxLM57{=puH~dqO;js$+9zvg zXS+?<6UnWMFcTbhj@yvMIUT@+S0|2hSl2nt{q97ATwQ0+zvE+hH!f5pz`NS(n#bHN z+CR8!dzykfHr-h%58n4Q0^Ho+9#@|mH;8~Fw3{&B=XI?zXylmrP6=q;gO`Li_E)R< zwG$%bJn6D3RpZNZ8gW#{Pc{baN7UGQdq;&`Y^d76zX^Eoa3A1>Ho z9ioNj1KdF~g_?4s@6{zq=SOIjLz3J!F)HFgEEW#JO}4j=!wNi(hh;|caoSrkUOlkr z({HL;CG)jROih1nZzLe}S~a;M+zP*-4r1INt{}698f)ua5)(XLl7uYg`BtaQv6^w} zXZJ^VK8wL{T=eD!oY`a)na9G~OOL1sLI7Swd}TinmeyqC)Jhk-jd{Gp?&&jz-=W3# zY}54G9k7m^ir;jC!ignT=f`%sZj+-dcqNxZSO}j;P^%~%HLP|)*B~@+cL8RgYw#y; zQD(k4-CD!T;>h=$gq!3CF0|ss*v6N}lj8GuKup^>1f)7saU?S6jt?!6=r%r(2OSm!Vq88VH6W$M ziwK&FX}lk>Td?L1p(V_dF8kpCUh=H>yH zA^#x|IS!Gic>I`MxVsXn3-}%p1o$AyHV&`aj|e}KnOij4Jk*gh8+06@#w{2xQZJS9 zrenU{*!?&FxUAgSP=mb{^i3$0mtQuezaU#MRF;67g7f>uDTx=dR*=`TH zoJpM%s$cp2wxL1k^j$Ri4NydeW6%J9g?KjFxV9~p{LS4rSvnW=jgwr0u;#*I5D-|I zw8qy({8Ahgz1s!V_T1YZX7&>Aqox9(|D;iR2Ihj8m`Js~!XPkFnS1blkDlv4V#dNa z(;D1`lnyzK^BXvp+NPfDO((w{+_WsaSGs`Mml|`)e>G4qeQ+Ud0p`-S`aH=@5>nQ-91b0QMX2FKd!4I zU=I#H8kWu(3u9;yG+Gu24BL+HPaJ7tVs`pUko=JSUMRHt$w2_P7UGF~;}WnSoj-~S?}O&Wa_0#|^5GN&Cs2BT&BbQd1?*vl zjLQ)q9ss9Er*UXUfO$7!uefIrK#e$_J>@SM@LI%I+;h}-dsq7fX0B8kOYL4@j(TuB z7y3d)KI=!|A~~fsASUOb7DKdNO?d)bBw zB!=~$6&oE0AWIiC@ce2H6Eu)a?}W!7ha9T|!YLlC=X{9F18%f<;By7NopMBa6B(@| zVH7~r%~=9VXF)?R`j37=Y55vZN!$-{{Kz1W{T!LSQ;G2L<7{JTSo`pmc zMk9k|>UHFH`EOz%sv{|_zj0i3@hp5Rhljyh$WMIn7=uGF2w;V6GaQGDC!}@0N4Wx(O7J^`vAIM)?~CkuleePDbd00 z|3QN>@p_@fPNX;N`Xkmoi{{TX9^&U*!tF>+6qvDvA~==|xNO3l8rUn|nm)5$WpSB_ zJZ#@IB{FMfMVAbQhCAm^jC=-SszoZ0TK+ca1|mbx%G0ad!g86-z7W4|YtI3Q`fPQtrN^fVW^AL74Z z76&x72}}JJ&k+pnw&ZAOZ>ne`Yx|i>UB-1Dl|wE*cu1gbuo}4}4(Ka39%KU0!5Wv( zd)r$Rs@Bw?j=ag^Spv6A{%i*li530*gayedR|jJ}!q`%PAlSZ0@slYwBbh}T?v~sF z_DZ(foe`kgAUj>zBF*C;mXoXFkEQDB$_w4fo1KXN>ipx|H}{=Y*_D;!j)(F6osiSR zbd^f(c$2w^iO z_zN0k!V0(AbG_1r?@n8zxu&*XV4l}K&6Ft*IRZLGoOn-l(;JXg+hxxFe65()TXs=b zCo4=Ei1||EC(iHA;yeVLN*V6Ox?iu1>gJPBGY9*#Gy5wBYsK>ER0*cJ>~6nKR($%X zwSCrTzjsqS-T7aN3Wth+D=KCUQlD!Hp#eD^pl6$3lo&>Zmwi@LER;Kp552P8yR;}$ zZ21yLt4JrjuHg@-+(b_m>0aQ5*U0zAVm^9N-Tuzn}fF(#V+3@TqxV2be&sJK- z(`miE)zzv{EYahqmM{P2emNYeg#dceYGPo$YoQhyNeiviv^D%e=7sc@%tSulde#!S zW5cH%23iFu`{yqz{SLa_f{xrwDBh5eP-1MG2jYavyNp0C-~yuUo|4ZU z0SC2evzeV;!>Mdv6}wF3%Jo=X_PU6_c@BqIuju~BX~+THOQ|`=U6>^dR*}LD;|2~) znI%Vu>~q+r-qos07brZq?R=3;t%IZ~hr%k7S+|K`~rRSDf$atLUYU$!AN@p z%a#Wg-wFJIv*Y;L$p^OYeV@efd4U}SgtnjEE(sHELKBlw;kmyV4iKpVsmU4>ISEiB<^NaP0+uGB!O=VbTn zO5Jyh>2)!&D5h-5q?@LO_sVC=Q7aVlnG0p=U~rPx7t~X4L78 z7YU0Mt)z~F7u%yM_CAh4zHW!5KPlKw%A@!G-mE`LteX4KuE0vOuIJvfDS<^VK)yU8InfDvJp zdoVAPw<`2;0Hc=o)cWbs!=+r2TP*!Uy8G>gN}q`Ny;1!6!I*F>m{oNku8i!5{MWGa zwF_`DZ-%mX0ySe-FJy+8Hiy)_f$i3T?I`BjW2)S`C$m(RZ$P=2n9z&0_ujm#+=IBJ z_VAn?5|;Fon%0VO(JwFtX;`m5BrpK3^l&UBrOQ3Q0*ZAGFBtZyCbDyFH*S$;AC`EO&qZL&CI%oQj zo&E~Ae1P=T+jj;CLOG1CC2}+c3T`>&?mXsc2e!Nsw`u_%H`zl^X4RjEvG(gXPxJ%x zY!1E|VhD)5d*sUbu4tB3=EClI*-WR}6ugWg-{!8triI9K3dq9}2{KcK0Z{h3d+Rt0 z^!U5>9v#_XI->0KCd=n^H`O=KdJIjjSn^f%qVD1T4;GI{#2@inpZYl3hSJEM+^Q?i zz1Ciu{1I0~;E!JkE5tA`p2uR}wXW1JPG`1IqW5{yR(q-Msrb?BB?LMH#UM@eQ_e0~ z_*`y0oyc*dcEYN+Q#y|7r;iob%7X1ye?q*roiGL9UM*;KqVj;iz3K8FR<8{?aJO== zv|@9}2m0SVwJb}$^tKGd9YY;U2^4_46yHj4AXg2+82@IW{3D4Y6gUl34Q22c8_qTp zZ6>gicccKt0p?c?dCbck4sTxllkU! z``ty#{X5be0&wZT4qkp80lWEQl}hWp-1nq4`d^-RT*qUP@&=uqyhG5t9o$@Z7TiTR ze=f!YM8|edUxBOfQ23tv1tW_-Qv{rDzUiQ|Iu_2;V}%NzoF=9+@LhbHV&p9m3d)}l zpU+-_CK%8YGXmb_=|jk2N}zYW0Cv3jtruaEW>RnKMCO6|+DBho5Dx$YfJ%tPoohZ| z=&69D*4ud6nfo-0esZT}lJH#eLay(ul*&H;AJ!nop`(1(rVepVtELRh=#P6wrKpQUX->|mG z|FnViKeo{-li4+W^mcx&&)=ndTie~Cc@-IEsZUhOc+pWA^o4RS72{?2Zx zan`%~y(ya6&XGPB)z?+$p}1#z%8P9Gy?Jo^%@EdD!yHw>p-fy;wo`TjjrRx>8qhs zW-#{8jLwq8ktvnab;c0ngD_V^0jaJlUp!(&JYXNg{eokS_nxl0QSYZJs`aK@ zj$|jT#+4o%w|+TH7OTL|+HLgB8>EIOA2N+doba8XY=%Y=5J90?zX3K$1al<5kSug9 z9hk@CHfYmm&X5#whj1u-uW>7IJ?(*;HitU+iSnEu9)N-Gl zKM#Fj)NWnwoszhId}taVb$JMdyRlnKAQ)@d$|73L)~&u&sdBEpb>m3OTHdG_vf34RCBEG)6NQt=kf!vJB-%~R`2Z3Ok4gfQ zyj`>wsJpT+w$0^CXF6$-Mj)A(e4#t`$V2Gmu-WQb;8hZ*6YDAVqo&(g6pI5VHf36r}H$hk}_>_fV1u2{)Wssvih?o!jU#do`o?Ovv8O>_*J?7qZaG6Frt-!DSo z8Z=FQe@#Jy_Z5-ypLg{G5%P5V)74StA$|>n&u3MSENmu0zR!SkCXQsrA;iO@Fr$WI z(Q^t*r9?*1n^ISi=SYlSN58CeACoK#P6^wpTO!dr471_v=^EZso6g8EGX9E-otx)vWubOt*5a86M0jU;X$af~UiWFEjJFDvlfeI+nM z3>4v36jMRO4s+*F-wu|lk+``&jLj{Xxr4j0mdq~DHb|>n2XzI*FvRxGboPe@9zXV= zE*vBt`7Ol;iXR7YjIdOA9Utsr_bWdoG3)D(oS5+kWbH(5zHVtvg)3UF;BC?Y_FgeV zLr;GkITs=ZHNuEmc+IB#BOaO!2}z!F5h4zkL-g&=38}UuE;o)?OrE#Vf$&U?Bgf%F zi@!8C9MuG)F5{;7nXyGOo*UCBigk6!~xWs%W%TWH~>yVyNBv zXR4UzVye26GGTxdXg|tZ7~-cm{Q%cGdfwwURa&`={QLVSXyCL8Lj{>|vvjcTGRESG z@mw8!#P^{m2Y*C+^;p0cYqgR8QZ$x9-+Okp&g`|TlVyHdj1l8(VT%jONfc2w7i4fl zU_~!4em1;UcTVL5#hC-R-xsf|Z&=Py0ceZrgor{+j|GE0{OvF7l7yc^CCDw4QvSrN z{JSUx>s@zFYxlFU><;@`jjW@QRO9}a3@=)WRr$u>Fk@^hJYPI&VuBMAh*VH8C(MK}H8sC%kJC{(J}^-i8n zq7RO(WPg3v<`QMhryqFLH9&61WV#AD-4rTn5Uz>gIP%M541gdkyo9%)hgPPNigR%fz3wr~4Tg1VeU zS69w|kXgb=-;&ouwqWP^tGzhcD6(k( zIkplX>y<9u-;b|ns`W_&khaSuKO54i6mVy0x7s5gq$()~er_<4)mv$kElYXwyjgu) z95)cYPCc<)e03um47ZcoyOiyk-b+IgONQXnsDH+ZK_v(6C6iXs(t_Fs|AP^EXtjn8 z&c%Zbv?hGsmw-e&)-p2s!iv?{9Su4aQ|u16I&VorxjTvab$rxpi-j+mRBi#Cw_woj zd?Q%Nf~Qb2L9aK&cX%&xZFp~*h9q9F)KspibhX;?j(MZKTm~{~PJQiVP6fulC)7iZ ze714(a3HoS8xA@+?^EjW^~!vKRF-m)Lae+PmEO4VVy@Q2LA0ARSady_xY~9w*Fwek)azRg50+A&Y&leG1F$PxDHW7t?% zZUO5d9#8kk!voKS^7S9uFbHT3Ytct9F|iaS{SwNTu*u%UVL$XV>8e`Z@xS(B`PevagR_`T&{;Cpei^Jv-JU~hzWwqGc*)TyF zSKY`iY&4J}?{61h)lV;(J|MLJQ+F;d5G(uv8;Af^^oC@f!$C3>8sKEe?%W#n7eE6k zAMv#J`kAAOpAiCSR*S0+Csq%f5HL8+yguCs&H^!?|Nco}XA%A5CMq~MQw zj`zpBDlA^O<=Pj$snd+{$qM;&9Al+Y*cUUYaKaTK< z7>9-sz5S4d$B=fXjDWu~p1Wl2z(rCd7mUCA5YjG&{?T!*(E#VxNV)0tlxesZ5f!+U zKiL8R7Qn|CK^9`o3?ss~H0kL@6gv8 zk9(o4e!fZIgZWL&{p}4${(B3bsSNYHI?o^PGnC5Sjy@M-+pj-Gf&UF!?Rcw<)hGGq zoqW(AfPaG{#KZs4BnWLtsuRu)xW?KYJ)vi_|MSTvUcNi$aDT|?;P%w0?+ZmwDOO2< zNs9qohrBcwoYdcs{q=A!0BD&q-b(!a?!S8=4@|%HfXPj^m85iUW;jArWU0WguHPU+Mc$ky;f_vz zZYrW79@^5;40xRCKuH4Z9^ayMm56`(*=-@w1dr0#~xe z!XhUqxUO^(|4e|+2j~EN_r>NXZ$p9Jm0fPaAZwH{BC)fr^B=J~9a&)O&|TA7&ngnM zN^IzW{p`^Q|5f4|7}bn_y^0RzoWTA5H5@sPr|BVqC+pGbsf4&JUy=c*N-zOhWv@Ue z5TeyH%^3VWdTT*w;^4scPX489YNB)0Z}&$d_ZjWg8a&YS zoB8_9@E$WKQvJ>SId@Xni}qW^(6N#y(MgjTG@r9q%0F?s-JC3YLxifW?|!j1dtG7K ztoDi&*(#2;nrRD0tcEm`=aeh_b!?aBU&hfoY^7Bot+0016lc}~>AV>6`vd<5Z*65#!!>sX52%>6t& zKr?c8btK!!=L%drhmA@i$aj;Y%H*(!`&BwsYk{mxy#Xb*_f1D1=%{MQj1a=|dF+?R z=Hw?y@H?0~OD%!sYlKe~HfEE>OBxRh7my1#{I7)M0}7p$RqsBvXm~ceo#CclX85Rw zCG|=YpTUywowSql|I0qmVeq9O`%6s(++6R4?dx!;J~g6|hy;HTc-rq2Sh@o|KSe-9 zayBN&W{r|BLgUq!A%RfGg=h***fQs+H?f8leAzib^-BAy?tMR$N~Sk zX9Y4qOp?o4b4B<8YRdFOcU^_{Sd{_&z1u3hApC!TW0Vu(9G&*#;9}2k=O`Z0bnzF> zZlMze5yh^Y7x+OyW$vq;tEM_DS>Rl9kX>qgYF5%od$_{seX|$1f%n)kh&>`(PVNy5zLmXZzcIvEx7i z@mSZbQ&+%ui7~HjXN8g}(cuLAq`>pNFBnDRS!PGXYA3SwDkhR?-yt(^kOWfGq;|7_tW1hC6SMn=#>j-_AN%0+^1`I(6F5!_jcSN&I@;s$mRg zw1i-n%_Q{DBhb`KyWul(xdvwe@_Aq6eqRkjSLNpeu)FgUn0{BeI}0^-a<~g11E7Nf zztdvrOhzplPi14z+3$wi8A)duJcvi7`j5q8!T#X=iXape=+YyJb} zS*p$JnDQe&xrskcZ(EVU{&?IAX6tLRgJA)klS=(T*Yl%U*oHIA&LJVZFEzxgf`S8u!FKV<2}#1N<*I56 z_@W^g)M%|9cd*Qv12Ob$hKvh-guI?q^7)QZC2*)9Tw)4n2^EjqYbbv^GG%DviNy)1 zIbK_9P-9JbAgt8yZ1v2xnzs0%Oyo=+&u*UyCk3vEq-Xlk_WwIrfT7fP2Y=bC$#Vy5 zwYd5K)r-rgd3MD+Gd8QlW|#BVEt2GMWh$BkIs&IIq8Ti#zbtsm2L^__%J7t}&znJ} zN>qi&b9;{XT@k~3(AN-WZAhr41>SPcSwV5yJkN7fm3*}RQ(5If(_c?D{>Mo1a9~`8 z&(qgG0CjVdSo3xWXf}M{cIAP~A@$_pjx8*`5eMMgra_n|0d&AZp_E!B){md29dS##5e9;Xycs^F`Gg6>KUj9C@{+;BB4juC* z<=^fctr8we9+{^%2#%w>Q8>K#plRfx$@y_;BnNpyZ_sOVk~t0H_yXWE+Go8YY1_U) z3yJFtq$v{LLI4g+^ln3g6fCXf2|cX*l<^QiYFe`m)$RM{aH1fR-Bu^^+YPVBX`wAv z;$)SZ*L#ZvAzHAKC8ziM9TwQ6L64@|ET=8QQFg;U+oS|&X8oRy8I#Z`ZX=#_GyZ9Q zO&BR)J60ZbO95O4IPbM#d*>8ajiEfiOf#>=q{2we)mtW?dPUg`I6oAWTkNhK`z#Es zOLRw(QpjIMRpI{|nEnY)u!#RAD6qy7pnY9WuZdHps0&)}88z$RA_;GbjU@t-=zk3R z2En0{AweVxIp-W71(KOfEM@GahT15Rb1l$i1jBKq2x?fhFIkZ%9|w(x5&)9!ORH}m zEVKIU{SCTl9HVz1$b-o)I`Q%YsdbZ2PgnEXwJPhMpA9U@9-_#k$HsDmME0ax;;jTl zS9P!3!N+PXDmVVuZ0h1MbmSlQIg99je^q+he=sT2tp)yA=~w$# zK=e_pJ-XX>o&ngHuh=Ry0lwLdh(a`GeR-+S&_v^cCsH+(ZS^2kSvVA~NJM`+lSlr8 zVq#FByMG96Y+ev5soq$MW)MC{i0RoyNg|c}c4Ax4*z4ArW={^Xu;Qe{A(cvG+ZTys zn~Hz1s;dnR)&#jG@Uqdo!K^n5i`{u=n;fB zB0}D_cA>$>V+$WKKqUBre8%|sJ?6z8)wuuAcLSzrIr90^fdDH7qZQ=>(Bxa9S4L&N1Z6-5_EqQwUEpXe$^B+FEPZ zJ>W!u=Su)Qn61{6$=BI<%ceg5ey&&-(;$URtlO7eOro2lYEK5BLTuNmUtrfSPX-G_ z!Ts(Y(Yvl!9WXDJ*lVPji%xp@IueJe-Rj;OUa~k4++ex(3ni9Htg+h{J}dwGU@_x8 zn%Ix0GM{{_C#R&0K)H?L!Zs|*O+-brBF`|qF>M=@>AZ?tsqUVbn~BLxa=8i8B{&)u zh5h=IW4EFZ(HOc{#R)&l@H0Z==_-03_IJ?B`lD^tKa_SZ^ANO+YIGSMDEg6WuJQ|hahIA7f^*rHUQ||L zG9!7Mtw!>UB&5k+&LDd$9#qNxdy%i1N+7c5o@LzkFgXAB~L*jr%}Jd{P&_s6xBA^=_8vTv>J10D6F)B zn`tB{Wuyw#-&O?00HBjp>x-cAM3s}b@h6r;6ZC++bEjh$YvmU7mF;XbDb!_Hi}zm1 z|Jz3XAkdC-qVq1@At%BZ92|=J4y*Rwj$uZ*u*_f3oxxBNcjq{=`x8XPOsI0mm=AxU{>$XdbIs?G_61>j=!>J286`%CqtFy|l5CQ0uN zD?cb$B-wY0hs!}!5Z9-q-YH;l+LL>#At z5~0v}4n0o)^ECc991ejC%+Nyq$^V?T>=(4fd2T|75txB}gZ$DZnGm?WcSfOPI&=Uo z|0m-Z#a9SC?T45tfSvDby}h%sjPK_y5FE&m5r2HTBmX9i6=cFLl--LWb5FC8@-sJO8QI-7V&L zTU~`#w#f*uY>t{k%ft1_8eHRaX#lpTjBJ4ir9$bFtor6c>hw&T$;+RFya6Ithbs@i z*#Pi>pCjz#>o%a!uUZcuqE!4vG@2C7*l1JR*V=v>dD8MXI3D-I*l#$)KlsV+8Thh& z>MM#|?XcO5Aw|H0&WF?(o6Dq&L*Uc+t2c=+ygEtPRo(@>>8@JfLP7&sX|?iUj||Wp zU0E$fU0ua&v)lXqELN6G4?CbuBvuRv!+{w2EH*ScpThLoAC+dw+FlKFfAVy;;WQ(`D7l&t1pZ9Qn{es_r( z+gwZJI+Vnp66O0qcwtu3Mp*UvRm@jO%vE{-WnUN_npmPr?(_ zLhFNgNbpnMfJ>a|aOY#8J9U*|;S8yu^sl0_FK$P+WLNpIUxYjNtA3>_&*Tlvc3g$Jw$VcuS8dOC zJEKqKmL8Kw*MslHm0h3K-@@+yqXpPR;#4m2tI+1jz4D{(Pou0D$Y(AJ+20M>QFkg* zN6_3En4Q3xE#lV573tgFC|SXxt)<@cH2|CsVClrg`=og8?Z`8`YH?4Pa%^Z|DL!hrzHnI3Mh$B8pkz6_&IyWI; zzwKoIgtR|AbQ4P$cif&RCY6 z@*A@y3c(X5kK-5kOVzKZ)N6&m=hIK>HkyUg3}_g)hjECelAA?+Xmx)KW0<0$hBIQO zM5`vzG?5+5ueZBEH2rQ8I1rv|G=mk#9D2b zrJHyty0U_r;~d4==&JPP8YN77EwA-hLXE@X+pVp3jUIW;J{RBCJzY9Gxan8<$B2(Y zqX$RYwJDFWBM0i+)Ag;k0U4E^cHROF*Y@{Q`L6^8fg~P6Pl>eBuKiEO8!A_2LE1gl!8t8g5DI?^L#)}ZVe!f+cJixCW8&GQ&j)y%$yA(Rk>UCqs z1hAqrU|_L-r=nj{Q9*gY5k7VDiw{0g+{&b+IwHcCGZ>L)S%H#$L*;`5wbm$fZzjcN zs%%}NMhl>;Wx6<>SJ!-h;MJ~Dpj%aDCJKLt&|Y*Xe3rFSg{$qm*T`;o)HobsZ^kkCiGD z+Lem^0w^9GD1gkw@*bcWmTte=vAe$I_I10jw~oX<_QHGl8qOnJo)3oXz3wC*ojUtV zUtvk%jopj$mFoT@!o@Zb01?G)|E|2l$Po@VjCM?GkP8Xqw3xiVYdb_X;rClVBVq%Q zfDf0NeIBLoitA8>8C0{&4lm;eapsHP+F~$-SmyRF-}>7ThrkW{G!A=rP$|^9ds`w| zF5uTOx|MX*;1@gOwawLb4@+SC2H`aPKtay|4#k|l8HE0XHl0cVEFfU#}A;%W&*fa&i2u@)d$Ps_gNvB35zdrKrnwnr2G6J6}&| zPUY`91kzpFZ1>4@CZ1y+SJ#;aS(N_sCDo2+F43&$zcn-#t^bpTM$(BF5Po2W8tQH- zF+%E{)P=YtQy`cFip4&S;p!z%313Yls{-d zm3O{4<;W3o63Dn08p~GaRjepKAHCu!P8^JkD;n2Lp5`gb``Cq>H=27SyWX$);=%2^ z(64BI(2R|FxqQgg6{oL`q10U}7g7pZ4za0``OTtU&Rj>n6b zzq4DV&K6Y|`7k))z%|PP+tdENA^qE!#MKVyRsMF6@gmMuxwgC&y|FtimJCiP_uj9p ziOT4H`DVeORen;s9KrAq;0$`@5@b;^%}IyL#@T&^-%adt*ssfvpVklwg5O-ajeIKDHQOP3!3X|Tv*9GxWqU#wLT3j-wJwaaA@_nGL&e_;er(>1L!n-{m zl^Ye+_eO~OoVKb6QEHg4S4Oheq5*1PDCxZ%LL3gW{t5tH`GM1|jn{KMbS@sT=n)%8 zl7c)A*M5bU^d}b(88XfW$q@2pnXgC&VQ?VU64Yl^w<)QzBhSJ2`1*JIZkoA>jieZl z=O)haX%;JXj}Y?4;Y1x55HCPPF(__U8S_8>7|(zG?ViW$zA!DF97((8jhK7^Ldw?6yZ`E=$PcvkEg}@@|a)2Y7BQOH3j;pMccK{yq~gf*%qv&6O)ZmFQ)FY4%M*2yT##`AW^?@Q8cyM9zKY z_NDv)m&kFyUL2!4Jd>!uEO0$G74aFP{I){1xt{mfpWF0@YMIN{*ucg4V3=Pl!9#?+ zwaUH1s&=w1>*%BVhKFhpWPbRTgVftKXL?)Wb$UKl*i3D ztx{;@3JYy1Ief}CwYY}V`zFTKv@y>_35b_c5g`Lifzly~Vrr)Wq*N=ndPW6Oa1fFP767N2=I8Y9iw z`RTF@4L7F{No-Y+?q_#n%)E3zy+1oQ+tC7cg*HS)83=Xty02OBP`EtZ1ybNYjBHz` zTklN%cmWjB0}z1vb*%TobDHjVkzH|w+W~J#I9x=wX%+M_9k_Y6+=~j(g{gfg3}k01X$8HG&(z@unB5#tLFpS>QTz0`_m%< zWyGPf9`JMT^D33LO}8df;|)0PN*U*z4gxnVnmDF#>&|tk!E|Kc&#T?*ey}Rd!I4^M z)Jq3re{PT&WC&002N{VF*m$Z#f>&MyO$jd|RK&_Zt+s0;!6WrAkNYa?RGl z1Mv_0WgL_}9#y>o3S8qH*_}@Rbc7Z>wK_D7$lbjLM<|Dw#xlNXS=I zOQKnv5YwJjNdQxl^N;98RWg(!92`dWL$1Brk8%P6K#Xk|a_tylRc zghkt)r4iv~2}-IPSUFeTpMnS|Gj5O8L@smnL)8k`Rk3|Vq*p#Lk^Vud!(5s*F=y zCH@p*h{0~MXt&Mk$?}0-E6W|rUI-%R;Q54XgXs0J`f6aRDDXDD5teJ|bkmPLKqBsI zBLQdj$CdyZ#vb1)b-x?2cIIc`+6&>lrIO^h#vuidyMC54DCiHJ$-52UPMRP?e@L9IMjOcwXnLB3aj zNOZJP)$N(KFV-fd3rVDlteU{$y8TwWd;QFbJC2(4_)-*)O*3Ym>~z6RnonUVro5jx zogfEWx@dPseW0JjT=|NmKACGuocF!jh=xCk*ofi6@k==w+e=#kB1WA|UQY~W{k{%A z2fQg#>&Ed`q`fNZ{iR(>R1^T(`ly$xKz$ns5177h7;yS%$@-|(D*js;0Pl71XR+S; zC_qj(%*Fov(_eB0i8?9IEn=d2V`4RMe0A*FG?``0H!i-HZDm9Acia{Q@;g<9fVJWz(J8yd|ZXl zhq~R*AMjzdma_6qgmsaj35H9fUqTK)EWcZ_kv zpE+eAtNceb%r6nx$$-c=ZwjADCbCuvjj{sQb5`M|l1jdx$Uj%Ns4t-iszx;d6cdo`UnGFX%ZnQVM z{f6C`gt!hsLtNHF0prr0+VrJCQ%I|Sx~h9G1R2WUgbm%25*obZ9<5O{+K?fxqgl-= zdy6-mKv(A=1sKd_Z&6QI3*IzymMZDz{^-{p1u)HT;Q_U+r*j4J4VqZdV9?ku6q`WI z=BrHr0%a+sdTYOYzp?ee4Gs6fN`XXwfA@laQFU}G0wV0BvqxdD31Gk1JKERBs3`OS zcrp3XCHEAFi6!3s<>gQz@(>&rvmpWK2oXTCaI!gxD)JeS65_5Bdu6Wz=hbNDU*s>_ z=Ym53#N;@v71P9-uJ?bMMEW>sJZ95#2hoCf6_XH3r8VHs+HOw0wV*!6mmtf1mIUdD z#^t~S&!6;PK}i$ve*KuipxFj888?8J-JdGKrix#BSv2mfbCJ-s%Z5r13~VXNW~R-F zE8%BqK*QjZmH2o2NBd@%GsG?F%=Tn?k?0B?=FbBu=)fJ8N{_FJA2Qg*ST^*DsjWKn zoNw;mmTNWybszqybiDh7`OWd-n+pzi4TvD0zA{+A)p$%}n4{U537hKI9}&l;Brw_x zqY;-|%aT?8={GpHa@8tWbSYez&uLXFFaWTj4#i?}t*hh;mukv>iFAy@?L-hh!bxn( zbX}AzI-lL+ZtstA);iA@A@R-*zG^7q&(B9_(6ayH{`0b)&NPTF^#%m%2QxM(RTme##Y1!pNUA5%~Nf`dT{N5famWNJi zHmT0l>dHeZ8_n$~@cShv=PLqak|`Lu%B4sIm<0P@3p{cC$_NU;17&xE)rAY;5mq0oN-EkHmWGTA`h`CUq%=Q7;N2w-@PxGwx0T$AF zv7xRYT77Li$H7(6;Ee z_s=N`b`B_yRbrWx&%)@O4ri^4EKWZ&5l)Zl^BEUQ40q^9-u%TD2LK!>U`OuXnJAyX z<;TaD={z%jG~I`*FPDdU3wi?NJ}3rS&m1>A0pKY^0!Pb;{w%1^6FeaN;*ck2O_@sm z4LVQAb{-84di%`Jx0?fa5-2CBO;SMx255&qN(m0jMyE`nMa-A+AAZsrR}Wj|XCyH^ z7Wumf<{$OYXJAL_JbUC2IMT#7)qiSNETQv*0<9Ssg5I(sYG`QG#1AZC4#vS=gsux; zjM-hR=Kp!`Ju0A>3Ty}RaR@5)pG0XOFh~4!78Qkg| zuJ+1K=Xdg85C$;>F;B+7te~apc3ai`{#21sAq1`^D9LBl$IKgMNZ9*X=WiYwj0w;u z>}*i_-CfPd7gcNfn57)9O8jTHx#XO~ny)jrDu^DDb8!`WX2RWU4ze?~3IXyLmHB1Q z%jp*gNr2cB(7OLue~>)Xbo+}B*xmX>_|>PIUNtfvceWHTPI@H{#UPVcyJjtY8+Of> zgKP_RzX7V4UXlSCr(e#-(L5l-R0bu%XJt#RL_5w&WwSc^(2^Uz8-s@YOSQ=&;(SY9 z_~Be@aPXSG%7b_zt7GdTl>C1*qx_r}Fm0SseKrQBaBt?-Y>{}h{)A3~vrBLn)KMyq zYzcmjP(%=MiPLh|7NfBi32%szbZBpE;`YIt9$ziNAwXNh$_MQz1Z=bN{LpmrXFoOp z55#S&4MJVVxADiWFH@JUQEvalbvqERQD9Tl*23RJ~QW_*3@(w@dtL z@PrKbt9?NMB8*FgutI!s1G_#;oq>HlcaaL+c}KjS|J|2o`CK3tHF)TQW@*r~wU{S* zSFLtvkOG20Y_YAvKHO-%G+#22ol=<6sz7`-$o!ehwO!Bc!IKy6l&UgXAE2evdGA9O zjDByfQXSKEf0r!-S(l8*@T^!Wwk|z25*UBp9EQ1njMVw;dHjZeauQ`gwhRygs)qt^ zO^fiZ4i2P$nA2G+0tR_UwLTK?Rh-mtXIS_o3mf$vkk3Ji@}31k-6S!Hpl$f=I~z4n z;LR7~TFl-MpvzxSu8%JdMkwp7>e*AB`(r7RZKFJo_sM}~$fUM^MV-n)a6ZPkyoV{Z zIVQG1v;GYIYeqm?w^s0Ex&ODHJ}J~0nzhnNRyNMho>xU^-HH^FRy>}FtQsMKvZVIK zh`_gOXtMB2Cx9IW&T+PGH0Xf>Hm09o`)rS|#+d7ci9a*m441~XQnueGX$vrsyTU$h6s4O=s&(M zOoL@e8W16?0b^)`7w`WC4W^D>3ORTZvd2fzl8Up9b#602k zXMxQBUZAkr^?uqQV0F-#Qh;8f(A$!|DN{x@LGS5`%;b%a+O8UXJq^CimgrgSkETIz zrT<^7y>(PoUDr1(h=c-%7NomNx{;7pIwU2fJEcKM>25)3knZl15b5r2kcMv^yq@d1 z@An<=e;?xv28VO@+H21>*IYAxbIxh+9P9b5HJJyVxd4RsgZM;ylVko}wN>nnQXq+N zK@0}wgKjb6QxnNgTYRWXvxLNjyYsU2M9Z(kgV9mwSNi;@6H3;I8{KJ%PRaD)0U#I5 z03V3tB*sKbRse-0foTV<_$Y*rtF5m+r*vV>oBt&#>;JeAxCa{Ql!qsRx)sJ+;x!!Kg<_>R>I~ zncJAW-8M)~^1lUQ3gln$A^mjw^XD-^v*&XmloZ5ZgvYN~lgac@Ox|WvLF2a%0*b)( z2_7q_oLKB)u>0NB-bCwC7=$ye!Mpp%Jbce|(eVLIA|>Jeg@8o$<<8MwHzZZ1*T>Ky z+N@_W?NN8}TC(je;(b!Rs%X zugOI~0eLHnz`m5YqI#D^HNBs;o2M3UNzuC+Oh+2 ziM7&*ks^#xbj+yGC2;8lp6Ir!Sdzc3%4GnvPW!=`=FJ5XN80L#dAR+ z=S1&3stPHIvMcky?~Vl&&6%FDPHXysjcHscT!-ayfBB5*rkc6KKhJ7OE0Bm1168}} zJ3&5*6c*&Oygm^Mj5&e3WHKB<;$NiG>h>$?07sm(-O#6l>Fyb26l4m4_TQ%Y0{ zRfCK<%-sd&#Fdx7>Zw~cyHv)!?n_7-g`9 zD;qS9uG*;s(O6XZPYV>_p9m-}|B2qft+!>~7y5|zO-P##TcV2s>F9V4fjzkjd7){i ziIo-hyMjg$DG}K%&MTgVv<6!c#rB^1>f&t|x36>QJTUVSS%;d5#ED&Pz)B60-sJx2 zAY`H^ft@tlg3nufqj;NA`u`;<3_B)~(IJYdvn&G^n$K-BYx8HG)EXV8Z)5j!DYP9PoRImzAim~PQ3K9`z=k3V64J2siHrk`e;Gz zOXnyWRv@mnEQUA~#Ta%cAEA?rwH!L~J)X_R^%+_%97ls3cdE-acf3jEf?VYUFT*5E zHo))myMC6^gwW_gwY?(WsS&p>X``meOhcG8**TA|@j4m*r?x~K#5SYQY3;p}E=xIxVGg5T@Y;K}%MfAvADlav+J z>SwZIO=X9qavV9&A@d5C2Z95mP1a)@w!k7U2O{5~dcs^hn z%Pe-krdeSrj`$oE+s;U2Bs4FQU&mr?g49^gY=C+@afO#?T8}rA!SIaO*^T0ePx&IlAp1FVV#4R3J5C1A$|H|HP zKgbL^`~n}LKej8ut+6wY$j9smEWROLEx5~2nK@h?)0DMRz+7sp&DaoOo^`)r=-B@- z5WBw16a0mmIn(;=7r?QZ>RJB43B1vS8G3lb zxT8NP@-v%Z5BSu3G9PwFed+t76*ZfSY%VXwlhS;Binp3HelVvYVz ztLe$aeZ6TUZyD9wPy*U$nsf-ddlb%rP@WP4UVjhz2}p6rf|O;)JDdAz)Q8cu7NSGE zT;g8;WHmF&^ZFkwz{x2SINknl&0R`Y9AGhjzV{sZWS&rC)I6BK7EDP}p%cpTY2ow3XwJ z-He!d@sm_1-UV&s*H?FR?2-J5Wt2W*(dg+o#!%_nm$XYSZ8}vcN!X=yZY|~6xrmzG z{Hoa0Ov!bmJZUA$DLv{JCk}w98;G;wKW;zW3mDE-*ZIwmK=?C>aE%;^u>wZk z_WYSEN}Ou8W2=nKvdaF9+v+iTxt7`oju#P?IoW%EvH^}e(-o{1!?J&96#^q07L zk{y)62rJ{n_E76n_yf>&0Nly=7FR_z-31MkB1rmop_r!;k^M%my+dL%lNhT!kF#+F zt&-%~%3SMuq(D zyxx~1-_|)Ic;9Oo>3^fo3{4O%+8PrH!ua`0(5nt>Q@6a3_pmytw2kEMuG8;=B-Bh@ zd=B?iqfUzTPx`9NLB{mj(dwCIMM9O9$!*e19MFdJ|LkYcfrosh^z|pj^3qY!4ioEa z=Iubzh-enymwbx6kFXSPqb74-#trhD9W!HQ#Z1|~_#IQoJw`)`X;3Aa5kT5l$Z zg}hX(#rz$VzOt&-t?vZPhj77h$AEOWi-Rm~2@~l!$xFsn=~U%L8B(dH<#Vmo@9yvv zMZ5D@`a6twz{CBOp7CH>ERhsNr;_k~zV3$=-D;PJ726floFSsq-p}@npZ7EY$GRzh z78lpN!^ZB!y=SE_ybRjCM5UIy{3Vtz@`Zje18q8+RG11RF&t@Rg(dzvIO*rkyAa*%Pc9Nh$GTr@+UBP@EyEx``{pC^5KW1wP zOF~jJ%X)+pj4ybHvzdpd|0s#CG#>Rq!8cT-#ydus!&E9Y{aodJl6S?9rh{p z1)0@p?mJ?5R+%W~dm!yH?t?SJR_n5b?N3M)QdR}vrCJkROt-lV6(k9d%{V&!NGo91 zkDO1u3-LzrbYZ-|?IS&&<+qsfK#MP)m3IUsZqTu1u*K_s_sZqZ{1(B2WJekBm}4AH zdtu>22t>#9pDb!LWN;)KYf>ztJv?w>Rm52$Bi<;w96Qy$?jpyM_S*%w3^JC>x7c>Z zV%K)!i|1FVcE6BQD75nZ@v*7RYWHd!?$J5#KlUateM+cyY9&BZLPlzxb{eOPbr!GA z?hAM&@-6Mz`ueMA39>g*tF&JdEx$2 zt)Nw<))tB@2K8bN(X1rE){~ZE}o9yr;ry4 zJi19bfk>nCqTOR&;{`hNW=-h86Ib+j9TgvTocqH6XU`-Zl6t^jjnr}d*SaQafD0Ni zOwb?lYB-EUak7eww-{0%^!DufD5U*A`D1ZVB~mo!`D->!vS;k8xf2}iI|@;=nLkk|l(Y-Ri4=~ysH70aNBCeQbLPO5)ybj|R z3RZi4$Coi}558W_H3OkY&dL#`Ozy?GmcZoboK*h0v4l|yE=0^81Ll|W*7~D1kD%I#UrZBU^QY9SMP7+t{RtaB5+zja z7MT*qp@vBYHV`z&aa5+oEtM_6Wtd>{mTw21$fwI)3bilEnc(!xFRY`8m!1SYdp-oK zL-R)pkpZibT=!C9VFZQdI0IiZse;mdFX$yPB)0J^&B#SA4solDsaF?^q?Uj4XWMf0 z%%D8G=kWoyhpE?&*4Kom-E`S9$Ebemi_8jXtConS_HFB4CFjd$dYyhZEQOwwhHsa` zqPY&1evI8eXHIe^>M-?a31)=R8H`H?RcPr+++un*Us^L#gj2{(%7GfyD_Xa9tdh1^pmNZ|!ai0@Cqo07QoNdO z4teSzZ~X&`@%_4t4`jYo?h`FgaHEpL(o|$}ng2v}=q_(1JKNaWDkywe{rQv?a&-(U z?6 zM3^bXKi6+|K7@3eqMqqDQ9$G!wM$Ts#reO1lQx2PE>ldE3#L|)Qv6p(=N+E zOwa#I1$8x1{h)UHAGUcIcT!(Wh!2mWZjH{zXp)h_j)8pj^$JNMuDIY|r+-#VjPfha zD!2w`J<$wljy*qL#Pt;UH()SiZiMN!I8zkXFI!Y|m%ou}kia|xu7wg7JynpuuBQtJ3|9Z(b>W2^`)47dnJy*V`kZ?3KnpYW z?g12P1M->yIbjUwD3i~D`g$_>99TZgZzXW3iJ8L+duGGQAMnr^5odzbQR8P5T$s@9KJZ&bRTP#}>Tk+3RZdze zaKj_g!;WJ?#tFzf{Wwg^8I=HUEEEj!z zKu$0470vf%O+~r7mOZ3QBL+bomXDng!8*nQ=t#nIXnxK=9|DJ}k1)uM*hd2c(L?d< z%r<}-psUdEhq$xrakqS>I*bbr^X6vz+J$7xE}Y8=xWOOs&Zj`n5qY>7w#bp5PFk2w zGB`rR1|wGNt)Tv4;C}p^w0RtcoqNP;at#bWwjsWw3X4K-bmFXk?{#!)45L8{pC ztHz##m9Q}H*hN7~i2u8+t{b71Q0yrW;P3voB%OY1cr)hD!09(hkN+nb4w)3{8A}!6 z$AwLH1wKW|7NLLr@Z!Yr73<`-RqXXx|BMdct4WYQpWz&!A)dMi{68Ud4^=aOU)|&L z=GiVK=Xi$qsXpGKAqXZ3!eLO3@}mWUmC->o5&BysplE)gyX zhKE#qs=q-v*CXx^ub}*hMHnCs8~m48&+#APa_ByD2w|-UQ_o`Pl8BL(J4-(>v%l%X zv83?tyOh*YkF5UTRAhTt%1pM)h=dDrKcFQ8YIxpT0k*&>kpZH>iO2HIgee$31(gnA zSNR`qHkP^t$tPu{9u7Y66C*DTx*#@G`MaY zIAp;tD5^AO-gEg+)SAw)PTfz>DDR(OV8rh(H5(^~8^LEY+7`yIr_cc@>q#RhY<<&F z%vH-an~DFHUm`91tpMm$$d1J+3j1CJkiO?@79)Vi_0}0>{Z*_7*uJ=LwDJ?Ae#e0> zz$eP?trX}2q=@_*&I$2+I-l&}8pjSd2X%dNE&xDwE!DMxxE9N?Ei1F9NNr(e4q zEy604WeIE)c__fskAlt6>flr-t;nAcpOZE5`e<>JZQzIp7ED1RLHxhZg1@)I@AS(W zV(H#Mgbwo_hX)=E0}ZmW;7fxUXF3E=1Cgtr#3T9apIQkxDZ;AIkeU~`V1Uo0!IcGi zrWGxme%k)D^?onu@RO-Tx<%L!8C)T03lK0b9SdW?KGlH=M(1^KP5n&SDB zR$rgFNUrFBEJ*wcIZQd&WFScXY7Zni{0hcYAt@ z5aPGG*zb?)`7J1^WUPhUzx6663_QYj!~z=#ocU#OJg68Y_^|IOL6Ws=NP^V}V^)uJcr}0>)8Q> zW#kVdI#p;*sX*)fy(c9C9XA4sKBAIoM2Y3fmmy|~g&8^}0Hs92%pUtsay?dg^)7O$ zZV`QKXJ8zPW%0o;(j(W1Rb#*9E-If=C5jBchr^5u{^2n5|KtJ{5^+CfJ`d?Owftm! zQD!U&RSXl(sK^*P+W{n2fx?SvPnJl~_VsP8Xi=YPns&E2lgBYd;N8Oe`rFm1nv)Bu zKtEYLE~cbddefJIe>mu$(C~Te;biU9*`31>-8u7>S)D%0>Mw^na}NIs2q7RK3S~(c zKz{VmO3sjavm=D)LJ&0E@)sM}ypl_l#`zflceMPWe^1Z``E7b5vhaS|kSHkM>90{i zN&hxWiSvB*36tD5tA}3?x%{`_jk_kbn9DhJuU9w=GhaThObEqBRt4`D`dd* z^q7eBM;Un81jqLR8(*#k!^hL!N_g{QE;KqFx25^#(9u^XwA$r&(2p}P1dgF*{6{*FKT}qeMU*PFB zdq6`Lwo@CdVOot}l(@V;-HgWbV{}U#4VXq!YUpW!O=gR*8S8nc@uBn>tS?+8wK?F0V zKN6-^iCQ#@n-!f=n!>Th_4{(77}f$=5Y_sor&MC zbtOMes23Vk*HsTexF6_D*&5v#a?2t0oaKhOp>T-REVaVV*jOi(=%e_c%mDdC753xL zB7j7c-gUU{HrmKp6Z`hJe2}5aVynw&yynLh(8GbkSwtST-SSMP&1dCJ`C^((iOaPN zT7>n91EHnr!zDCwGpwHHl2RaO@&5PKoXgdE>Y!ll>)>3}#wYxW`3qjKF4s$#g6@~W z>IzvTzpT+K)>drL_KEo5uT+`+u#OB0UXr1S^|)Uji6UX3qetyuHMZ!Q zJGFpVl^;(~4W4p+1BRsH4}vhA#(R)1*G5Z<&gG`85xD=rTlbEj z^C^@CCMef{Vrfgw*+8fb{ky?nQsXjFuTKi%BI*_*mPAXC4qZ8nD-!E6Hvm zRtT$A`n^8U+)jI8svpEc(TJK&-UQTrvL{Q^WN)}TL`q#RX5EY+CUxf?*clck{EX_d z&{3!@ak##c^T?nVU{3@JV&qo@oNk#u|3Klf3#z5=dAAIWtX>pYJqbo2#M~@gUiwjo z9m%P{U)i00L)~N4dm&gm<&4oEK_dzNtaCAFxaGD#bW{Nn#)lw6(DO3bZRI)qM>LFR`b*gB{n6(YiM1j~ zSmu<#u0+>G30eONrZlPr0rd~iT-_Z^C z-s^cb8gI3r=++f>4+TY&sy^PF%Kdhq9P zxTD{at}eqVlVddeD0n)Cb|!@rG2~{;h(98id>M?OfFa`l_`T!veGHeJ9VNr+yr>nX z2nMO*o89sdW7#jFYF0}Fmc)&P!A8=4Ix2u|%i}+AeRK@GTvH|I#w=P+{FR|xbt;)k z!jvg?g>oR|(fU*3ZRyU9${KMdebA)@2$Dw9YLJ2ObRk@C9{zdB&2Te}m7E9$1YlNL z7rx}vVU|ShIOQdy%Sq!2h1HGqM)9-ii#w{oO07GcUlwfRNJfi28Z=d7mVDmHP}D8L z+=!e?>Q_F`Z?QE2KUziH_NP4lv26JTKI7gmTrZ{t-xFt#FFIgDye2nzDAB=^_@Gnv zhERX=)z7BT#p`_g{IHyd2~8^zbc&=VQ~+&;Yc1$5lGQ6CYo|9} zF_$Hcbd$zYgaP|l&5Glj%I2>Ljom+M!MXP_qE|cT#E}d-3_W|{39e^a6)@-b&kFEBc}!Ne z;yh}73H#1(MF=r8!xTrFTN>ZLndxpf@0=5qW{HcBuLi{qbGuwpby-Xnh$UX|{_aU9 zyd3>5FG!?cOw}gQRk{~iJ;F~=>dh8{pnXqNBOVmGHkU+$;Hkmf1uV_~Ndr&#ubjE_ z=WR2mFZ|*hE@|HzTdO4_n197Ebgff9%bkI(CEa8(`I0XKLTn3|=vGoIU2Sb%Z}j#`?WdhU z***(R1Zb%GLhkx-=sW)OLdQ3A(3LWHE5;I`^kf!$&)TCP{`J?>ybo3148F?q!`&nR z6*|%@Se+JXxJ@zw9iTpyca)FG6TxpMrhDRG;l6ubA zvpd0wUEg41>2S}=zU95{x4A;^o9e7$G_hBucQWCx?s*}p!mn0MJ$^xm#yTFH;2O098y zPOfUDlwM7%ggXYJn*Ftg8u7NlI6N=2D~kfo(*^Tx%4GYR`FkwdK2+1?H;k@}}wswWc zov&gdxC*+qMo!e!?RQg>5{IK0j-tNnyfhd#?s_RnbWeN#HgAlxq)UrqDPhrCm87KZ zDjva<+LT%8nAAwtOyijlrA%lL{`pmq<#1oTkxYSGrLsHz4Ug^+Of8^fG3SJ_3!+m>T=tdO{Dx`lgGI7h;@R>l0kSmiJ|@8iYE zJ%i|JQ$H;k6C%UJ5M*AXRL{l(Hb~iA4syptYK?boeHeVMb(D+kp))mC^bqOJuI3v} z{R<1Z168vxO$=U9M)H58%Lr)9^9_6Ac%K|+zcu1M5s7=$o1q(Zr1-=U%r!S(*hf}wk_l~C33;EJ zkD4fA4PB9EiUs=OF~%5K>b$ zluU;|TIniPJLjC0cuTZu!z~j%pu9=h=SM{92|#kdASYjJIzSvh5DMO=)KVxne7}Qj zp^UNCN~KO%KfKt*4*-AoKc#Wx>q#8bcIsd2&SamRdY!X6bmEi~X~o_3;Je-*vt(Hc zR2F;n`Y5S8ZB}R)Y4AleVCL^6X?8kE{#t%c@kS)hsz>mgqic}WRWpb+VP?dcI_op$ z^IY=19L4{^0$>)*U010-KA#U6U+R0@EF&n3o!RS0kU@Qf8D*GlG8xQ>)^!)H+QdT; zYT`fx@xf$Eaup6^5K8vr6i!oS>b)7@x0rT)6kmMNl_eH}gB{zC)jcGXjifr&+9>&! z$XBy?-Z2J2K;$L`>P0D(@UCaHt)|>LqsodhL<(s@4@c*CdN=LXGIX4hVMSv?(W8OX z#rL##U$RUl$^vEDp-=xzw-GCW-=RnucFoR6(Ykw$v6e^5iMXtjb{OLWgJ!GQP!g3b ztgQJ8z9G@%fS+yJi{_!$_#X?uX$Q;NBSFp^gTKzN)mO)}C>7~R z=PRg{!&!E3jYL2i2UE|VR-H;YsLGR8Bodi*V0ft1GC(Eln!%qir@`>^!aO#LCYszJ zLPkHdKgLqPNGdEa;7HQ}J>rx0ivmmCdmbB%en{#_k+j}1i3!%tcy4~3=V94T?vH$% z<@kp1MvDE@+_wt?*bEGQXDv|!G*|->s)WMTORM6Q28!}h$qFO-D62f;HLI61gIZw$ zLwC)iQL14vLN!*dlx$>*QB+mA-{0(^F!a2G?f5P`b)^zVX?jc&U>hXddjLPqgfJ1h z^_w%%FW&07oGqs6V@%%drJtK%pWp4;tNJX4_tPH3T!)SCi-Mg(zObkfMRkSvU5OuO zEDo8DB~rX|yIICY)vo;xdv0WI3<%h&r8pn~Wo}YxCQ+&l-#1EeRF+4a zCp&Lbm@D}?^v2V?VNjuthdY8rq2lx;!pmUZo`x%%QPexad*|6ppJV-ud}>Y-UZ#tT zq?Azc^F4(Lk8)ph=TOkZw3Vb?R8>mTUh^Bf3)R_#kuj5~?o-vG5oM3n#OM2bJ}MG} zFM>_ql|9mIOFLI68(lRnm?#Bm&c+*b476 zAlPXj<2!Ox()lVZIqxvYwxtklzn9#t4n@9H5Nxg*MfG&8)V=}#2}wCJMpnl6-8>~dhb+1X1C7;Cq#}36 zLDBjvy5ZS-4b(`o*J3$OJAW&(nChbQF`BZyEn6!b2xhrW_ZC=vwa*DU?=-mG!dKG8 zm&)AurSbPTTfLsSGIRZ*%*-06Y=^T3p&SfeRdY*c5sUlT`v{+eKvp*&;GOD~WH z(dZHHQ$vA7689>M(!3NOyFcJc{3MqbK$UE~F83RG4H@6>CR~s@$*NQ_2)rmamhkr2 zt5;zPD&t`jflXqbJ!>hSIQUI!QMM}Gcj^_G977q3dLzf-s)5dHAx=SPl=cy&0QVMcDLZFMFSDnpr6EtDHd9nmb=F%#}@c+be=l!sq#F_QvI6J6ZH~LzdJBH0TCfl z`fO(!PhrEf-Y-9H~UTf@&bu?`6@Ke?Y>F*fH^3$36s61wwhJxFE+&K zx!qVaLQs07xBCiS ze#<_yEjJU=O4f=6sN(7;{a=UPv9N22j&)fE#oq4^8qb*7;AAP#(hw(Qi2U<7GY%#y zV}GLW^tK-pF+$x499bFhs}yHclqUrzvZokpufoxU9!e(epl9&Q6BSM*GtlI@7Y}SI z=GD9#V#uL2?>Ate@zs!OF-7uMbDiYQAiSHldo(k2dvAEw@P+=fK;C5*t?}i<&*Uv0Gw=3oc+OuKy zSd98Tce$NasuKx?iB@+zN|FklswCB)bZkK5l_`(be2&q~7Q!`H~J%U7y%W z;OOW1EYRD7gx zE+*p8u^z2^pWpWmhZ7|t2ar$%+@JCfLcmiLNQ7P4mbBv%`+Ef!Hv!SahS?&_Q#5+Kg{Woi+xAnyJZd3qYZ z`U+8YUk1OrSlqE5I+HQ*mmI$J2CKYS?F(ejAh_-LlQ*s7m?cBvYnR zz_WFmGnCNaN{p&KvJflPm&KnTIu_Bb@9MJaiiCuD6G#(jkl>MZ4S7qZ^0ePTYoG~+ z4IuXYO<`jKSmqksxxoZvkS3Lg2uusc)kiYngXr9T)AZ~{VSNeS5Iu%F8uqw*H|x1v z6cT@&5z@o$Cw@$VVYC11X~&mn*iR?UEHWg6I`6^|Aa6-kg57&!i+sdm4WLhjfd&Zs z=P~pz7BVy)K-D*BT*eLqTf=Bz4|e*ad~^VyCjiDje!`X$+S$gVBwy<$fQ)|~Z#Bou z{^L`0H>&=6n`4h!NnqbSD3ylQn@Bfo4qzGKA4W1Y1s2ZnlNcdM+#j$xBuLn8hl{@o zZ#tnvOQ5m5@;+{{dMI}Ml>(CNMAY(~o=d(?opg05bu4=?quQc3D!H0j7pqt(N4z}m zepAO7;_6~^j|zWg5zrmZe>iYW3tH+F&_j}E;|imt@9F7?i~q3E*bW z30tpjB@`O{>CNI8ETW~Ah%9_#KY_*Irib}B$gg#A&Rez18)%WpmLWa0%qeax=bEcP zf{+wX7x=o*Xxra6S6%~G3P)&y(~-}QgpqzSrZ6LNg>J=CTMm;h__x)>dd}fLKSSRU)sYCh^8%CW{EgDc}^^dG_p#8XxoNLiOg<#$*TG129T4bp`3lgr#)htIXjX(DI*;1aa zE=?|ZY|iF088hpITQ9f|H))`N=7~2wo(^Uj2%~p8Uwl8wEP{NzKrGra49pe7J)rdm zWWX&KPngVlUEztdb+3l{Suq>IV<3VGia?^4j3!@FP%Kiw?J`2jw0C}SXdWJc_6-4nna&NwF z$UxFKfiRJi^+I6N+`CNU#%ylK8#JQX`XYUTr_Y&-jOnx#vlWIcz_en)wA#E@8m=!; zxYCWfT8u3nX1+l4Qd6R#f_X7TAik0chMG#x+7(Ut-LBtF%2GEw%{vmVsCSn(!~(7? z=PAI0-K;-2m^Ep)UZzpR0E81FN4`pLgl<)CY=wL8 zQeqw+=bLu?7!}M0CEAN)l8WMc=jX}JMDlECQBhRV-XJZJOlOum5QkCP z8!zZ1dBvUBxGQ}?Mr=EAC?ul=V zpA8o}K6lvSfEypAK1;0kOdjg^zTX;!PXTx~HkLgOmI! zQ3JX>k&o6|rH>awzYOsL^T40U=YOov$;Y*ezja`9jdsSPO_=d1MCfyLCsaLye;Z>A zG=_(VL&)GrA?tAlrh*(Niz|M=X`}I(#yY6I;?l5cBa zNer-FX!0jY--k51dT3~lOBVkA0fxz!wt)HFdmb#LAIiE+U}%MTuUm5*@Td|pTYD! zUTf93qT7*zeIF+Lp~sI^Ea+3s`68>J=T+c2v0$bJlA2Ix<#(ZV-*}jhAA0?MkEnaH zAJsPK$JLIVr!xb%fhH*ZqbI0yAa3_v zq*S-h;70dZQQE=v&jCS-K_N7y4epB}a@eC)QL9jkDgK&SS3+QI`C$SpRGx^S!9zm; zde)TS!!#x!XsUFe!)6;TdTq6BJqmX@TL{4wa(m>4Lf|K>VNar#=1VvKKI{5r;j}w$ z-ss(j8sqk)@3^cb_-?1fNY%!BMQO|w$9mEP6kV-5XKLTxV08}JcL&EJcRqCziRtmI zwyspUF4qFLyT*WOYr8WmE?l4m1UL%|``ST$g;In5ccS&ToP}Iv>pgyE3lJAfn4ai` zQ`&-TeWLx@I-m8kK5|f&GIwMADH0}iYpl%1Qj;C}uW#2#&(y5c`jZP&nQNdE5cCZ4 zW;Jg-JUvr8Ew~eX1tfn~R~zMl!sic|Bs!SzocY7UQ(u`VF&n9rVJgR< zMe^?VydV0cU2TTr-d935^%t+_K1I5o~!0CMv_(ZZLA531gv$qZUhGZ7- zhyP6Wh97k6Qv;Rj@b|R4>I0t=!u0v=O_mu#-`L=S83mX=RrQBD&@5iJcr2h7E_h*$ z$<$;T_qK7?LZDtljfX1D+&JJugGvPRhcX{kBrS7sxGa_-o}Dj&^lT%A=@aF)a1eY6 zmZygNfkEou3?TsBwFCuRXIWFG{BT2x2NIB8|&J+~7i1=Q9Cj*ea5L@sCrVjua3Guqz_Vf80;9;jd*lcq9) zNKl%>z5c5<0~8%_4~lpn>tTm7Jd8*H9sGE4m@p<5qy*a#fmv;s(vJi13rGN$fnJ4Z zwXoy}<)Os@@Kr*fd4tja|CxtyG(rIuPN>M0qoq-ebV2_bcDb(t2J_#7${GA{A`iMe z>ceV11;kTfDT4wMF>F4-ra<)(2Lz)vFdNWV6^{D^jliq{q!f9>odR0YK&~47d7z)Q z^g`YIg{|t^;L04*#iQT&L}@xUQc)msiaGsl9^D=MSSXfZKV2)Mq!%_se|S_qz(c6x zB!#F@>ZuC@g26BtKkym`A`hT!4_+A@bS8+5fTf1XhOU7U86nIKmH;saWvCmhHX+Mj?Ah>-CW_G)2 z`$;Ka0E>quFVWc0Z-q_(Z5mAA>l|zYXzwblPL8&}Mq0>0lO6ya^xv0-ICxO{G6t|I z|Kq=HU_PR_=JJ9M#DH$|hu35<*n+Bt8chGZ|NCEmR;XDsp5aaaCC|UF5^S)1MLlIu zk^R3fL<1;|3SV7vL#O!fOOGCKp=P8}?f?D@`UG8IA=}jbj&1*Qa*{2oV2moVemn5Y z|N8++D6r!FOX1g6{+%!M5APx{hJ|?C8g#k-eF@`|{~v6zxCRmhX1U|zjH&& zhR#CS{`md>-VDlRPS9Bx^hf>oEM%dx$Xj1+|KFPdF{B2&e;uZGdzY1~(}v90jyWTY(4)_n&jW9 zrNn_XGEEMHdL;gX@X$kZGM?+;^?wi@bh-uGeX&BWI~eLj{rBbV0k+d_f3h{BN5TELEk{e2|D7)M58!lAJD62_VkDhR*8@un z8DILT`0vc2f3(md1#+|FbLxzFES2cIdMZ{EA5eHU!(&8V{_F2ZS8z-}627MgJ?wN6 z0!%d94}$KnI?W${&~yahJ%0B253(`|)M~J~aa_uR9s!PkJxk`>+y#Nej6h?-K}bJ% zB0h|c*w^mtx;as)A~|{T?(cIVkg+)R@vGqX`90=%S|9@CpFEsSqelQvJM|(^(a~(t zOe4PCrqIes4-ijd+8T78Dp^C7v#-1_=^svpaT2g3@Zxd65Jpo)!VU4mfG4(`Z#@qp z7duozQi5IjZ&3uN07rb!QPyketCov%(vldkc(kGdeayu`K-92Fx81?=hRVQ!hoDQ7h5@aQKh>bfU$2@DK6sInX4=do^2`+YA8zEOtpBk|$Zj^WSUrUQYkk}zCgh0plw}o!^`4;yE7V*XLpXRx0-)SN!&oi=2iSVFp#}{8ktM5Kfg*z7D81(ukx?C)# zC3Vs$mGaj8dEpBhMJ@pY^uoN2W_P{`W?tuQB$*K%yT!B)l-$8gzRwc&XS>aAsV?t|l`H)lP^6%3>f8Qx& zZ+kGv2+AsA?A4vVy6kHbIlYKPpb8@l1r@qzj`|{K>3Pp_LmQqv)tvn?5Q}i`AyMLa zub|Al6r4oK*c{G-y)xj(6O-^)P(xD@AgZFhzLG8LaQU%=Asa)Dr8}D{zw?xFt#z8) z!rJfir(mR7HL4so#!*1e8$z&zoy7(d${IzpbSt1~%f;1NLDHuPBN@p4+-HWwE7JF5 z4*RT#&xg{xfOloY1XO1=|NS&4j9BE_m_s2~QYxlE8FS)?wrV<;=@`D@Jw%K6+ z^oY(HbQ3HwpMfXfslksnW)vU6FdC^<(I(()fE4kb3eUP6vD)sU8TQ6K3sF7j07jjB zkpmIsBhY(WbopCv>HD9G{3o8lyeOI(3&TPsmZR^Gx5rSQJc}1Dy82n-)G3m}?e<7! zVo&@tXp{(?E;k>K7^WS!LexuF^Z?;qZiXKxpQno65Hmb8=2Ro9nZa<@EW0FrGXHQ4 z?1Ff=p7nAAF?7p(DyJSPdk<{^B>EGrXVI&=8 zdA(RHyO0O;yfUsZTX+oIDSkbldiDj59dG3 zSK&`PZ-w+IWD0LHusxEBqLR4Zw=A*W;q_wxh2`GGrt-?UQ9gTFaCElA#eqq}hdV~2 zCXCb%S{m7$Eu4Iq`NsRwcrc7||Ksn*d)z4qef)r^c6Hn)->NeuKj_LMUlz%!4L{rL z{OpTa+{+&ES~88_Z={~7TKUQJvAf(H!azG7Rcx{4CZfxL(6;P5ttvw*8si6PO$a;f zEl?qhcaS9Eiddw&=pohe5_Ve8b@y8>jndb;7c~9e8qqMJ`yx2wm>?|h5GwGO=j5TLbg;IpLc>pv;3Dgp%)PD*5@ zpwG|g{&KyT)ck1(MNe5yTc>ov0^xKsrhU$r?m51Du*HThE|wZ!>{|1=Nd1(QOwAuR z3;b%3oqGdqgWi(1hkPubE&sZ~<%mJ}mV!!5U$iS6FV|RvqSvkvwzzXRLrU}8J#Cf| z3Jh3Ts+WC!i5UqGhaC-M7uupimE!PhM@^X~4rM6a^$Cs2GPV;zlW#(){uA@5sGjy9 zuk;MjlXr*i$MTFvwJ!TuoCkc6sV`E=uEVQeEjI+s7A;fCHu}OPBgvNXHVg`N%i*ZQ ziImGxF*f8Ioyd`i1f-0b-7fh#f9QUXf@p;&$dJ?HR_akI0oo3(272^#3F<#Or1SEDMuKebx2s(}zx$5u+ zuETMRp!xB$z4|{Y=ka(xa7dU-8?sZ2YnO2Ma}D`UUQv4nx=k*{P8JwGlOLC<_Gg=d z)R@!ACH8N&-&wAIGfb4uRmeg2v|K>!l!@(5AH7$Y2xjQ5B!gf$liHD&V%!(b6%`W$ zK>#|c5$jF;8u+v?{n`*-%(qY}M=qQ~Dym@5(&)7q<`W)>Xj1bzLx6}XxOW?{5z;UVlJ`n^o1{3r%Zv~?!^&i#>e}}$p)(@ z{KP`aZKSPJjxX3Hih_&jgx^s}>C=q=&gkerdTdrPUi{4WyMIU z+79Kv)IKuj<8nW_{2ZI?9*>8zJ@-oLlRFh3yA*-nz>A(;kUV9o-slcPG~4XgArx&H9rsM{ql4y>c!b<@-%$4T5_>p1%o7e zdb`W#a@^v_?yUmq^0S^e2R`&tO(8n8^*nvN|GeyvnLO7 zAo%a%h)QDFktm_z(80e8Akuychsn6dD55P6r>u_S+!zegxA_Lo)+LF;K}c-vwQO(E zrKstfHm^vVmXk+YrJS)YE)UFKw*4{WM*b`*vM`rPgRy{OIVI2RMD$mYh3bJjxG;g2 z01gE$9_Jg=N3QSo(otF&dOeE84ZierGzqoro)ONb!3i#8Bv^u9$#y>yuX1Qsx9dx^ z?tE?tfhsmPK!z?Af%eqn7>gMfInPyg8F<3VsW!Hk3R%G0pHLy&Y_wDd+7u=w+zOSX z=Kz<{Bg=J9HjJA$P>I|_2`DdxUXi}gY6NmIF+C&nO!A_vfFK5SRi-xp?Ns15Fj^ox zoA**feUAJH^9{}=Bpmkr_=0?|q5em=re`klU|S}G=6*kALWOA-{y@|PtqY6jQGu?e zqyeqtawb9sbVLB&>EE4eR90^rcUrF?Ug10UtLcFnRd0-bma2nN(jq|gvz6+63{^fn zgrH;5t0)=>rhUh?K=Eh;TBXnrlh1tg(E0lv0-7~2Qe%ex z-~#p!@0H3&ra*89kqIgO0R+pJPA`Tk+@FR1rErG38$j-YNm-*PIIdp^6db+Pp+&(d~SD zmU1}+a#pYQ?AO1h{?Y&b$(|<=<5$jm+zaX?us`c-u`RVE@_=M_thBg;Va@AB@^(=i z_clA2JJZ!9ADi6-@4PygjmCs;BFG}^?623WRJjak_vV`d5dyKlgABO2+Y|Dx9O)t& zJHMC12v8>|fMW=^svw1#p@_L|<^DP_4<76eq~JHWY|y1C0JRM55NRiRltH>}KJ!{) z1x8aUgf}=%xr98Q7rEW}Nb-5E!Oux4=q=(Vh(C+UoC{!&1Y?Y?-(q4FG`82`coua%9phSGK5JZm-6}F^!MN&W)$**nmbuC_qA3^Cv(**q~sc z*9#HRX$f*AVR`$)&?dO289V#pQ3#8JxGQ2dHzQb()s5X+TSR}nT6#^ z_+32KpU?^zAZMna5mdAs_vlh_Q?LuUCkN58Zy`y5bT4kcKv|$gnQqu`5F{Q2((I%U`NqKauP+G-mt) z2U(Y7$A1%Q8;d}(p3JrSaeQHuatio$UA<+Qcf!WBeV15 zk!b|0l6vg^a;?Ij-{oOWql^{oZPL+5ONR%oyg&B2GJ4rl9Wmdlk0AElbxKWKzteeBDDU-+(M%e4`1;5{P@dp-{xao$uRaG}et#cPTa;}%b67*NfsFt%Z!;C$EH z48k8Pcn;z}2V=QQih%rhR#)vsBZ#m2P5bD4%M(x^v|u0EfR z;00{JL)!{}p{4y40*=h6IBeXn;LMq6y`>0N`_eThV*6TZ6kF5e0DPcO<5uQ@V?-9m zV4Ab;vf?oc0uSdOk^cI)R5wc$o(M{5m zn^l}kOq4w3nI}T;WjMOMZy?q~B_?73Zl#AE)pC>b0Y#OZgE0A?tq>x%29D1f~6*7>cNv%=uGUGXGc2yoLz2+dgh?*rh_#mtM9Occg4hpc+PM?B?2+T>6Fd(` zar0`r-Bv-a8z5g3zd*GA*@A^5Tk&BOv&WmGTeER3K%MSmv8cgK0(ph{OI-%X2)ip8 zUQ*rh^`8-mUM*1#DDgico)KYO1+p=_-Dx_a=IJX<_j}*$G+CDgMXJ_rVt#ucY!KjB zs<$pkS*Xap!1a(c&EkjS?#J0$R|T=33w_m#OWyUla>WmT-`PW}tN4)vLJ?mP&x!DN z+~@QF`W^em+LAM_Xu%&am3%5qYz6YSO_1BVYSz{9Eo;XP`nl~yPlSH*PxKh& z2wBel@9}wzQ`Nm0E0FeP!oP=@zT`hzyf3O~$cnEyO_IEb_s2DNf9Gw|F}~wNKN*=A zw1^e9wqKFH_9*+G#F0g~M#bf)wgkHsH{B$`~tS^N%9bNjW7RMS9? zw;7eLmx#g~=bzc5ImY4WdK_^co0IGi1E^8ar5hKa52m_8x5A&@ z_rFV)Zd{ru{MvCuc0_}13@ISrRrs#yw!`hFFu)wkkU{{g0SaxUpU(7dPB&;W#m<3J zY1vf}&oopm#ym3q7&n73Vy@o|=#fC}p+i*Z)ELQ#2u(cVpL|sazVlz?K>lz%VN>-4 z2ccwAm15vqU)rizu5S5kKJTu4&n%8(GShAS2TZM{chQAH3sx6FkDb`*HMS-&j_fsN z$n$K%1x*M>wx+fc&#+2Uhmi6 zZxj2I%Cscy*pAF$GDG%-Bf{^t$l2hijCg$7C{F#|c(p0NqIHmgfpU#~CB1zUicSjF|J`vw{f}G7NbYHA5x|Q@vNAFEOw+XTJ%) zWAD~%I!q@&P}BHEVLGE9T3>f}?NKA?YZCB`F6`>7*=4Qq++%jGIKguERqgSS=ELiN zCUNZS!H>GR(GAA?zrS%%fP1Rd*|7{F`t|aG)Cnx^q1qmI`v73{yCIGc(#J|gg}P8< zGJeN!q*Nry#~y5bdUOf+5AxUk9nt>YKPVNlP30CTo$2$tW#%h{c6@F7(Ra%7)MD_l z`edZkpCRnb@pDP^PZB2L7>p|z+ipwvMJV3-1km2;O-?+?Ck<5X2qhwg8i0RpQAN_p z2=;5$iH-j(>SQtzDFC^Kmwx(xps1Hw!7o+a_wh+Cz}Dl zO00ceW=i{K++khYXh9m}Ql>&gOY=}aO(rz4G+IEvnQp?;GfeBLh)<012g$B7Tq}~c z2v-DDccq$qfOUWnWrN* zvI<4LGDUZ}I>F*B{-8Wd_}V=9!^8IlGZ*nlmvjFx*lrz1^h3H&{m=H>OLk|+IOjdr4PKE>W$Sk8FNUV*{TZ zIs@>V>!gpL#2l)tu`%hxyxL@NFW{ZnFtR_r3&Dgt)a_v2*&}xZHqY`qgc_Yo+3oP! zx{y~GbpZ*jj}dPAyl?75Oy2|jDpZUu9v?BT+|0PG@*Rx^7Y?8MRmiSCBN6%=5R@3) z%wlVFyv3eD*XpcQ=o%xpDvuG3dup6#v+cxXZ&X6+7_qBjD0BPJ|9rpwyFZ3vE$fi^ zaB;HwFAA}^j{bxjqNK_#pU;i{myXulE>ega<>*c38}lzQ&ZZcz#A*g`wTC<-sy)K>Y0u~`ejjy{ zcDj@n`pRHhxhOR0i@>?ti!mx}BgP69Y-bA3(I?5(N`1qXl!s_5BBxyd={LrWKa!)T zmQwv}ul!v_soou$gQH`fmMDgEfR67)|ixo=vbicchlBYk0aS zsKdWqObZyVs*(R?zf+F!pU~HoWBbTMS;6?F*PD?7APs|2y36SU#gOS^i)6LV(cpqD2;Vc|b04Sxi){NKoJY0O7##1k zP8aSMyH6>^#D-$PS|%0@EW-aVdDHd&Y>@u^+y3ma@^kJ2al8E)Lap`U&WHPW5f1)k zQu(?!S93qkh7{s?V0#?we;*2{R&Ua$Hicz8o-GtWOcf+$@`Bo#vp|EfMbLMBJ_OYM z>Rw541XI3+DbTA~vCv*wbQ_UF1v zx4rfQi&5)ecYf5s5a4;rs2fCyzoz`iuIs+2FA~6k|0c!|i1(;HO4o>Z#tPWlgUEL< zuJ#?LRG7p`na(%GSc}=>TyCe?1;=|^Szzan9E-~u2f#6+fFRLtd&~%i!2Peeu58F3 z5SRV$Sq~)scdN<3Ze-x41KX`biw&##}Q5Yr^H7U++s6z?70mwNH(IZFdwR_|ATmj z@pISf@e)(IZA|@F&=Fxuqx|Xd_M_7A0cpWrN2<4-Y~A`}j(&sq;FF`mY?Ku+n2W&| zTmt|;BlAp0PN2iUdhp}i)Z1#?e&t|XgWONt5t-#JE^IIc0K)|gQhW6MqR_ETL?FEq zk`8wR@XTy;UVNS07=O$cdT`K|KXJe?hC9F7LSR1`9J?c5J|cWNP!Po9wt}A8>}YW$ ziDYB&64*iFVI}y%9E00zPDJ~_*oB#ml2C6ZMrK))#XR@y!V;9wPJm*JF#YN|NrA_v zB^7}l-03n0wt9<;EA~G)nN0J}x!1_xW-drA0;`Z6R^j*?98{M)jd~im+}RfOFlTIYlo4e)k3sWe^SzOBBbX0u zotMzXT?&9iz)Mg2JP>9Dg|a;!r`I87sssd4)SaB9qyeHQnu}qk7`($p)?%yDY1oZ; z#XrF+vTc?B$At%g2mDg=_ki;UYs1+k`@vIwN$FFD4{_icqdNuj`B7V-d8 zNxw6KZyNTs%P$%aRQdCpqZ|i_L6A}{jK}a_3^c++0d$2+$)>CvI(wK9__%JNdh2(x zwAX_P)u#`n=NtT$8rAdlCBjpi-zE|h3^ynW7V=oZx$pC3YY_ZvL^;5?V41(JqYuTD zkU4;9_s5s}{t0A_=}w|e)UL?_@acRtWHAsay=`bs1Ednbo9mVR=cI5!F-8jPcytob z72f&+^uGH!#e=OBf5@f+xps=`R(3i2_LRoC)y~!Pg!=i|Bg}@gWf;T)llZ?ua?qu* z>L&lKYJNBgz*=M^9ZPY=;$Y~GD+J(jewcmgjA8>@h6GR!G?_LEy)FO4@;rl9D~_3+ zCJ7H*Rs7lNey;#GzE0WuPfvcK5ENQhDlMY}-R(=MVD`ycM|kt^yd3Mp+iou&Q3gE6 zso;6t?k|(JbhtxRo+<8^r*Wp+Ra;P}`F~wR%z66e;I@cwPx3jP(dUk%43K=lh)1_J z$k|9~T4_M|M0K(F|6C4GJ*2?yB`$RHHle6eU;rvvn0{I8FG6}e?X%8)xV*~~qFJSN z%LDGN4_BxgzrL_(`oFD2Oijmt=e&e*_7;9>Jm|W|n|+9me>5uSw?dM0{@m$&`}8$xKF4oXm%$d$bLO!{EJpA_g_3(8HTV_En+Jp1AO5?3LGvp zUvCX2xqUd3ejsFHlL<5|Sjf@2q=Ar;VLcoc7>X{2bl(a<}aNLBAU>736 zd>LUd22(%Z0Y#MkyPpFFG6bly(P9|PuoQ4>@2bFZ2$Z*3LST$@kU1 zndd&#`285V%fS)yFL;vxJi0XSp)5XW--5ZBqA>wK5ECj(f|kU@1`wf$p_Qt?p8+e^ z!)tix7=0ds|F9hDb`b_OTgv=*$nBEg|J6l_KB$YFLshR5H?q$w~=;G3^EkC3{+0#uTXB>4-mxQI{B2+Urd1) zxLy7U0|u)th21zqzHUsB0Vn4tJn&Whr*$dtjVPHS>>SFNhI6z5{U`;EVcJhe$-rDr zo-p=&u?st^3jje>AH6_@*7){n$K$`S0B|7D+`j=?LEfOXR>$cecBq8nxd@zKvLs$J zUv4zdfeSWex*5X|eXnbvFaBWblFjAr0*ghb<84$-UIh0SlBXq_PkIz`s(Wu!iXuqt zOj@=NU!W!Nq4IBZKT8L>QCFLIxm-Vy-d&%we&BTvEBV+&wuiw_b}GaF;+`pMQN0=h z&|f=d;Uqg(B(voxHk~2D6E2s-Uq(LS#*}BK)`m3C6kVY>`tL(5?3zp zd}=&G*z0(EMCDgw>^aHw@zX(n?Uq|cB7n4trJX~UCfqe3u`GxWFbes+&{>)B@NjX$ zJ--mZImq`gP%z1Lgd1Geix69*Ru{&Ts|1Ra&4pZK4C%;d935=;8Se-o5691quK`+v+ud6+A4-DCN#jwHXIq?TCu%ag z3&EEwoy(`ecukYS!P!@LIpL&-R~lY#ALxo7 zOmi&>e6E4g1ui%otglJ@{F3|OS~_=U;7Z%xTZa=G%=+_~)MN4Rv_i|3mwNW@bhVJz-Vs9~xM zCiNW#e$}TkLK?sKg!9!@!uG^!B*HrWl_q{%hs(Maa{9A=xc5I3MX zRwIv|Ve!jR2)Q@f?9y^DsfFM;yFSV@8C}nvsTO)L>tAi&ngT$Gr15H=Net>M8^qjL zEb^4GDhSK7kw1Tz9$3uQI|_-NrW4p4=*AVo^{R7e6WiC9C^s{GsC`U3_C=`og#QZ5 zXF%d!Zuw=N`Y3MhXm#V#vD7_%dvD_CcvaCg26||5#-5lXGW$sbaHj)zGEBz9-1&93 zyRCv|Hm~}meZ-(YGcK+@2#0#;NVz}3Z$JW9qXLU(G5?AXM+1c1QE0P$Okzkk;> z5+~6cdgVP^KMC$q>)gK@eMY3PIi(;2VnL9kV(`l5N^;v0bByeTpRM)bRnncKf?V(> zaggpsZpvM}*+l;Z_uY`b9O-WZ9<4OWc|oMo$DLuFcTz+#KBsAKc)Ruarad$bUS3Jx zp{gCNvc;@zE_uQY%pQRBzcIP$&l#>yRM_ydqmUPUKJZ$u?HET(luDES4)1w*l`D~a zz7;IbZt`flTK%iA@OuVT_;}k*#Se)^ekB3Ts^2!eZ_+L(V%wR)I#j=M@gMpCR7eeuYf;uQcqd8h&!w6{x*q7*1Sm!s@c$ zZ8?kW8-H`sv(53s?d^iy#?gesMp7(StpyXwLURtHWVtYC|Ja>x} zZU?d<_I(AUQw4p3lE8RD?UJji2(EXJB~>@WZj(85_7T58>YR~)A5}2uwXtmR=i3L~ z^0j3RWy&{yN#0T3NkaEpyUv+g!DB;6D^@7sZMs6O&)1UkwU+Li8vh@1jbXojsbx zYQV{X*DWM881L=${ao^-XaeP{$8n6_NenZ&t<9Nr0E;HJnd^k`(T3Wlhb;)wh4iFHTEJBevj z%w6{T?Q7B0W5<00wVGNB5A;wF;>a9ev!{Lb2aBuHj?JXntZ221%UtaYBTgnczkPXd zd>bT9!6vx=?yREJKlVfe4Q}jffwE#U{13ZJ_U;7a#`mtvhXN>}l!+|e5e)V7RGw~k zcXJxH#qxq;r)dR~FSJK8TN&x-HM$*&=@@BGRa_5-V3KYLw`gLdWy2Pb!Gi6Q>0< zCSuN0nF)q<+L>pi4UksN3vWKp9!+vb&BB*UJ5b+>#wXkVhjJhm(iud47@d!kfBKGi>^Y zCBz>s1{AeHF;hN|;7Mc}mzFjk#Zv|@Mya~1v@Jqx5J=7YA{RByS4_+0Np8^mUKUSb zE{0~Ak-xE%bsxv>^j^TFdQ6w`=ddfy#GZmO&pq0Y*8XkWN&qoFHNnuA0y(ugy3*F^ z#W*RY9S^ir{lu`$#cuDNq|rQaM?2SgK_R(zBYVLo=@tuZ9xtDaTPIvzqZhc8|7tLt zOWHD2L!^Y)OVJZaef1DrM4Zf5v|^{ZJ+0w~pYs(7&#V6M!9e$A@f`jqI+e)y;&ED9 z`=1+(%%WbM8V2a<=cG>;IobqNfo!Pp2(G{o<1LBVup7`?l$%=+m&;#7_G6BlE?_AU za*hg_UTESlm^&t$9lpueG& zkBzOQT|xMUyyWm$Xbg-Epe~@#!%6SVc{d}Ta~2+-f7DV#A8v9nt||z5iIK+d`nEv6 zLs1z8GLXV!;gJ)Q5J06MRniz|cU`_JBr&~MAy>0UbMGn^ayFGHgv6Zp(c&h8i{)j# z1*@3NW#y~ikHlk@(~1(=P`B~!LeZvdbPhtM))lF{ zA?n1T^k0b@EyV)*Y}Ntb=+~@MArI7y34%OX9*~r}j$6|wLa+#I);e374U!6X?-zvc z?<3eEks^_~oylZ|m*I*>5#6&N)2o-22gwaZaU|?QbV9U0Ww}=Cjz7L(yOcRa)SUN8 z7FRDdy6RVBHv8ogOO<)_X{)IGu)lrhx%0R+&co)E+Ocv*g*=7W$0iHzD~ZVy`r^5B zPIX}KUN>CtSvbO-RlKTFR)QR#oUmQ&ZY#^|UNZSBD;dv@73Rcf#l%S_2DGdS&NO!YU^aK| zP2ZySY$$({W9InAU|7DO`{(Q&68z3%eQ^@6J{jkFCja$uqr-cKmjm#ZywMGC@tjL` z)Aj;Y*#lA^@;icK^0e@z>d%i}U}QTG_4#HUeHu8cP$ZeXPena%-pCcO51ctndp#Vr ztD*nELOP1zTlJd6Wh7+kQK4yQ@77U9#rl#6Tg|(1y$MwkJ$s$$2+sg)c0vR51@qr? zy(x8xy)OhWdAG+;Dhe5Y=yP9#z;1>Xf)mhdxpFA~hZ%gcKB#Rv@aU8IH)Tc{o>0!* zME|pA;wy8>oNlXvURD@p^Z2VFzW4cvA|&?39|Aj~Un1023iZ9Xp20|DAEbATXgsAd zXmKb|*5w9q$NHC5%0i}}^Z>jtk4=k1wQz~CJ}1q}!)402w>q>pq=W{VVR0f8mwg&v zbh%6YAqX|Mg&+h&>~0W&JGo#Kr3oj6jaRfj9xFG390w-zJcnxad@Pw=B%h6pBqtTd zyi97k^R7w0R7iZlqj8K?AG4{qDslanGKh`=Pf?D~Nr$(kwXbY?g%Uo0-~*W)1Zc1q zKTDt`C~we8l5(i-GQM_On|?5NxW;B-o3~!ikaxIvzBR>$KiYA=sql1 zAUJT|t`*hkoz3HNJR1DfG#!Lq0_oD% zKB;7fnbO~g%E}6*GQ8RK^l>!eiQGdfi8zKv$5U*r0u|-!$-cAP^4(mkD1p5@DZ<>M z51~li6(Uc%GR!q3n_}a&0#-vy+KGJ>Nl4}!ze}IT){Npzeira*7rPmgnGv68hU9wc z=GgzRze`Dy`opR3l#aC9Lu8`O;J-q^R6mTkZq0g{Sw-}aSotN}v+}HBv;_B$b@E>y zV#zxOV9~{?Z(F2zMMW+`c|^LH<|nvO`2~WX{)>Np?QZR$f^bi4wD1z@jw8hsUDL%1 zQ6I0uciJf7PRdiHm)*X-N9Dpvk7YQdSaGuTuJpf;E?IlM!(aDB4(`;NOxIhwX+u0g ze0nD^;QJ0FyDBG61vP==LzCg){pLXgm-EPaBg}eZ3o{ip$qzNKtQP5Ku<8%H9a!4^ z@}$HHX3?5nxlQKWdhmKB5$RGBQRwGPwS3OAt}r4zcJ)eS>V@7<$`a*FO=jn`_2gnh zmO#@VJImzXIN)V^8#SMN;Mg4AdKbDq4s72eL`+jd==V|HbDc%1p-@;KYt=%1rbLOC zT$Mg;d6-sjG*0wdOAT0?X%GJi{TWBGI~}tW&c`=ACOL$d~4z#7slEUR94`vs`-3vPwjp+)5)XSLx4HzhbAjg zVg1zhT?tBb|GGKtCv|C~J-HmesLW;#^Yi2CIow^tJP5M8F;ZkQfyS3a7jLEtfuGaW z9+Tfi@<}I5C$WmxXfHNpthtt}pGk}}Dz|Z#W{Jg1r?#{=@s4N1y&B1W_myLJyfDJv zsjk~&$C}EJdq-=(cvYoZFLpF-g<5=dgQ-ny-lzB#hygny$y3(=SYV%;Qkg3mHOvq0~7Owb!En2tatQRta+L2oG6 zeTkNHw*Ry1e4o4w5!=YzMcmR-hh)|7=*+=o@bebr(GqpVi~ac~IcnR!R_Ah|aKc%_ z=jAy{imN8mL!Kt-d51W_^Y0AoNrblI*l-bW8E9dlqhC!`#*?=>OFraz9NH$b(rrfk zoY!kfqgSG3FVKd(_|`QO1L+p1(zhc6()ad*AsGTs@n7xi|oJR~r`0 zwmmerc0lJ?|H^sV{qp8fU=)?YGMR>pxJNFUuvCw?a!UtJmzB;SW4DL+GUaM$VLu|J zWRhYL!Jx{wOedSqw3|;1){??W`95{!uWe~QQfqqQtwxZm-}g~=oFz}9_rbeh&B7G! z=bTJhvJXuN1o}Kw5UrBq=-WEqq2BB$KGk%LAnH4MvGip-h>2gVO{qctSxm_SX0zR9 zI^RB+FP)x@75Z6?te68H?iAS#22tRBsM8VX{l=WG|QKh_eERdf_UaqO$ymjDby4i@WG`C zZwh7ixeE`u*IEgms==ZMaQ*e<7Lw5RDV?mg7J4NFBAAH^@Z53(`yGC>PN?=}!aPIO zHpROjpI1cHNzWb>sbn|uFYGoUqEg>v#7UnLT3lJa*O&w{hG`o#FbD`h0Pc1G&!;j| zCM3Z(Fp=1!#X^f`87P59oJP4pq%71u{ox2heQGZSzb0x+t7SxrENf7DT=#fi<3ns1 zS@rrz2**JP1I5zgY^XorlM2jZq4qrN=o9Ws1M3Iy({a`1tJ^j1aRl&xPV=j^hv5rT zd4)QXPLh}j60O%J>f5#XCad#Kn}ZV@=|mk%Ij!cyp&i?S+3A@JCgn)_+u9}3-;!~8 z8wU;(jF%NX&W!u*Z%e>NZa?clqnce9#BDWpvUtXTWu{Z?OC=i`wFTD`RMIOgc=ZcI*8H|3In$Y@$EU(c^ zU^hPbp$-~Y#0ktZm~%qAIsO<0)V>1^`hnNq01F4=^B+U3P&0G?e>KF)kp8Ug4!0eN zf|H%V=1r}c6)Fhh@Hcc@goiHF`>gC-$^oMsDty&-Nc(s_2D7^;#?$VkVcZ&UQOSq) zFX{sc_zNvd5w-205}V4 zEfin8c(m)hr93r~B^YUzxMQU;4}522Pf?7J+GBg;aTJ`tRH{kQ{Eu(V)S{hJWrp>8 ziHVzRI|j{z^!h^bv}VZcqS~t)c>ADnLa3lo5JZlba1O5KoRb;VqPo6AeB@soSHp%Q z1LoIx1pTXXqsuiqRXs+#s|-{0GtpQ|tTQR%P=_)dKj}w~D!>MmjEQN5&*kbFWEY_o z*J*3^Ir`ejSDwTyDBc%#rAimNY~J(yF2{Yie&;=vT#e)yT&>irE4kfSU5E(nNhuCo zpw)%pHVOT7gTw%ro@6vO2%sRWNz_aRD46HzNCfpGB1^$%yu#q)wda^GQA+56@3vS2$^O}FX%Dt8!_et-ME@cqx@ll3IgH*iEN{|r@9}ug1udP z2yN&kq)dIvS)Nmenu)iKS+_T2a=;UK{q|UWf`BCa@afQc!`Ziz?)tN-4a;92Hg3+S zMJA@V)tU`m5BB+X=A@&-RGs^qUg-Y5zZcRzVpq^T9MQks+itl%3#4muGPu#b8_hc7 zzsy{@?!TToA;{Vr<>|S-7`O3AiLV*#yD>}+#rvQ>e4Lqap3iIb!(q8cgO_V;0*7Ra zyQ&kLOn^+m)85ZtvZtKRd1~AA@%YW$s7zS-{qX%f=Z_E`efbRbn%CX`sBd6Ug5k9! zzWb`OGsE`vsfa`F#u%Z5#{*BvM_VWuGidmjGiGdL9A+*ROLL5=*A5N0t4k45=T3`} zQ@r;#^Y_8O4|KE~yr(U`n(}MqYMnEw;+GU_%#^hKkh^ZjXtp~Op zyokR(EZDf=VzNk!XZd~>rY+=O`I@<*UZzcmH)lc>9sya8R+bXtV%r>P)p;Sb;?Zay z4u@5yciH>nHnR3`(w$GqX|n1-ww{siK1J&MPm#W9lYfJ4D}l_(7ZTRbsUs+Qlw=US zG5Rb(hV()KT1d!@^Wa5md^6@Yr&);OU=x0c#c%xY=hM9Qb5=%@*L^~V{vv5KRh0*_ zK}BPk(=&0MAGxSfHSUhCf05SC6tisS_cfU3#T1JnAfG%&#`pyf2aixEK=zD;k@xi2 zswK6)! z3n!II8N#8xYdpDaCLKzvIv7fgR=sXJX~t#PRJ&=pJrDhkK|GWC7Gx_C=t4opcs?ml zL?PhOvJPrv*6q#wvG=RF<7-9Q4x3~AbHDZhycB}?S=JZpe8(*4l`Ukq$IVFyDg20u^%Ay{!S|56>3 zOjc3AfreI}7`#P3p(cZHks=^t{(<+TSX#2HFGV2oKz9FYc=OKH>eD51F}(+%>T+yMl5&%H z9Td=@G?)YvQvU+52l7O<+JwZr({RArXM$TIFLBF@1I4otv!Z&-Eb}YAfj8TQrTI6Cb z<>G#&wNsl_@ZbS5rkJpxV$UN+?!yH9GLTu_am&1?tWD@YO(-qoW?jRu>%`yi93m&G zg@evR^XuXy!)p0iv*eQ}|JxkzC9u9K{_5yH%Wmjq?Wv1Sg0n%&$>*Z|Mob@c5aErq zfWx03anqRKnF9|DkU-&O2RoZ#$D9o=<)x25Wh5MQC8~ny)YUN61)2Le`XAj4c&3cZrIOad|$34pvCS zQ_$FSw2xECbac?z8k1y>&kZ-|#1UAdM_chG&^0O!)~NVgWeep}#)BwwP+_;IfJ1k# znmCn!hX8bi6{a|Xw`2#O;g(t*$-qlIiqI;Tl? zz8w%p8cBUJ&`LQb*a=3_6rh!%1i)hF zCoIsX%~)}0pVUF0?0>t-HlLF~jOy`ubR1Id^1{1PNq=)QUXAaTIZXBSIQ~@lACZbnjGDM zj3Gwx5K6jOkAn!M`lrxy#u|PA4Fk3muKJf)mX3^jbpDt6{QOH?&H^rGMfiw6)@^m_ zvHa&%R?+tJ>>*O8-G&=W9Q{Xvh3nn4|E=Y`g9xXY5&MTVp|}3-r5olF(1Rg+)I=$}!xw4fWL&@a`$n|F*b+k%p z;r9=g51N;TVI3^T5*p85v_h))oWsHbqSVhg=8m7!mMV60kf; z!YoPf`ZWa^jj=~!6Ar_46Wg{kbr}c-#2II$*~Mhjt-(s2zadr8qph}(TDPIEU?0{cx}L{6G&GP zaInM$b3gP5l*M~Jg$rG|@Swn>?l|beuLpy&|I%^=+WvzEUb9^p;X$9F-wA;41kwq> zhip&hyg@IX;bD@{&XcV{IV29?kkRj7MT6I&fCQm`62Y>9gKvay^R9r>g%RwOy7V59 zQu2W(E-1htP>lekEdv;9{lf`r(6%9X{dHKR9@>v)@ZCdjR=~jzfDe;0{*ZuPl@WN>w(AGns#N%L*9!eKfFvE;S8Eh;T3|+P z#lD+EDXat-N&X0m9kg9HFd#QJHrU+2foC{~0(4Fdz~jJ^QV1AZofH^nb=VA8%U^+9 zW-HIR>kBF&02Cy6Efxp{_uC7M!U)vnAs_+z6lbjw0t!fhjwJPN8h`?15rCb_zr9QO zAN>P+z^p(Di)@GrUJVdn)7KnIfC2;;?Ye?#^9nE#0f3(5p_NYnaaUiGK~^VNPC*f} zZzD^B-q2n{CmA%68wMu#>xW#QzdxaU$pU>5uMx3=cHnQZe6ST2bMR=%|LZFM2jEZaowRWA+fQEq4{F}8Bme*a literal 0 HcmV?d00001 diff --git a/Documentation/px4_hil/UserGuide.md b/Documentation/px4_hil/UserGuide.md new file mode 100644 index 000000000000..dd76d3e3a914 --- /dev/null +++ b/Documentation/px4_hil/UserGuide.md @@ -0,0 +1,98 @@ +[TOC] + +# Introduction + +The HIL architecture allows you to test the flight stack replacing the real physical vehicle and sensors with a simulator of vehicle dynamics and sensor outputs. The flight stack "is not aware" that it is not on a real vehicle. This is a powerful tool for develping and testing code rapidly in a benchtop environment. + +The flight stack can be run anywhere that supports a network connection to the simulator (with sufficient bandwidth and latency to transport the sensor and actuator messages). This can be on a standard linux workstation, an on-target linux image, or the on-target DSP image. These modes can be selected based on the goals of the testing. Workstation is useful for rapid testing in a tool-rich environment. DSP image testing is the closest to the final implementation, so is useful for testing actual HW operation, other than the physical sensing and actuation. + +## Px4 High-level HIL Architecture + +A diagram of the setup described is shown here. Note that UDP port numbers are only displayed on the socket server and are left blank on the socket client. + +(???NOTES: This diagram needs to be updated to use control inputs over UDP, either from QGC or from other) + +![SITL Diagram](./SITL_Diagram_QGC.png "SITL Diagram") + +## Requirements +The simulator that is currently supported is jMAVSim. The setup described here requires PX4 and jMAVSim installed and running. qGroundControl (QGC) is also required because it is the supported method of providing manual control commands. + +## Assumptions + +# Compiling Code + +## JMAVSim + +### Platform Requirements +Linux with java-1.7.x or greater + +### Build Instructions +In a clean directory +``` +> git clone https://github.com/PX4/jMAVSim.git +> cd jMAVSim +> git submodule init +> git submodule update +> ant +``` + +## qGroundControl + +### Platform Requirements +Windows 7 +Logitech Gamepad F310 joystick controller + +### Download/Install Instructions +Download QGC from http://qgroundcontrol.org/downloads and install using the windows executable. + + +## PX4 + +### Platfrom Requirements +Linux or Eagle with a working IP interface (?? does this need further instructions?) + +### Build Host Requirements +(???Notes: Windows?) + +### Download & Build Instructions + +### Installing binaries on the Qualcomm Target + +# Running PX4 in HIL Mode + +## Starting PX4 on Qualcomm Eagle + +``` +> adb shell +# bash +root@linaro-developer:/# cd ??? +root@linaro-developer:/# ./mainapp +App name: mainapp +Enter a command and its args: +uorb start +muorb start +mavlink start -u 14556 +simulator start -p +``` + +## Starting jMAVSim +In the directory where jMAVSim is installed +``` + java -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp :14560 -n 100 +``` +replacing with the IP address of the machine running PX4 (Eagle). This can be found by running "ifconfig" on that machine. + +## Starting qGroundControl +Launch the qGroundControl application +1. Set up the communication to the flight stack. In the menu File:Settings:CommLinks, select Add. Enter a Link Name of your choice. Select Link Type: UDP. Set the listening port to an unused port (example: 14561). Select Add. Enter the IP address and port of the PX4 Mavlink app, which is :14556 with being the IP address of the Eagle board. Select OK. +1. Set up the joystick. Plug in the joystick to your Windows machine. In the menu File:Settings:CommLinks, check Enable Controllers. Select "Gamepad F310". Select "Manual". Set the axes/channel mapping to 0:Yaw, 1:Throttle, 2:unset, 3:Pitch, 4:Roll. Seletct "Inverted" for the throttle axis. Click "Calibrate range". Move the right joystick through its full range of motion. Move the left joystick full left then full right. Move the left joystick full forward (but not full backward). Click "end calibration." +1. Connect to the flight stack. Click Analyze. Click the "Connect" button in the upper right, and select the connection that you created in the first step. + +You should now be connected to the flight stack. You can see incoming Mavlink packets using the MAVLink Instpector (from Advanced:Tool Widgets) + + +## Controlling PX4 flight in HIL Mode +The joystick can now be used to fly the simulated vehicle. The jMAVSim world visualization gives a FPV view, and QGC can be used to display instruments such as artificial horizon and maps (if GPS simulation is enabled). + + +# Debugging/FAQ diff --git a/Documentation/px4_hil/docs/readme.txt b/Documentation/px4_hil/docs/readme.txt new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/Documentation/px4_hil/px4_hil.doxyfile b/Documentation/px4_hil/px4_hil.doxyfile new file mode 100644 index 000000000000..411b308be58d --- /dev/null +++ b/Documentation/px4_hil/px4_hil.doxyfile @@ -0,0 +1,2403 @@ +# Doxyfile 1.8.10 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all text +# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv +# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv +# for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = "Px4 Hardware-In-the-Loop(HIL) User Guide for Qualcomm Eagle" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify a logo or an icon that is included +# in the documentation. The maximum height of the logo should not exceed 55 +# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy +# the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = ./docs + +# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- +# directories (in 2 levels) under the output directory of each output format and +# will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# characters to appear in the names of generated files. If set to NO, non-ASCII +# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode +# U+3044. +# The default value is: NO. + +ALLOW_UNICODE_NAMES = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, +# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), +# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, +# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), +# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, +# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, +# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, +# Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = YES + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new +# page for each member. If set to NO, the documentation of a member will be part +# of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 4 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:\n" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". You can put \n's in the value part of an alias to insert +# newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding "class=itcl::class" +# will allow you to use the command class in the itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, Javascript, +# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran: +# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran: +# Fortran. In the later case the parser tries to guess whether the code is fixed +# or free formatted code, this is the default for Fortran type files), VHDL. For +# instance to make doxygen treat .inc files as Fortran files (default is PHP), +# and .f files as C (default is Fortran), use: inc=Fortran f=C. +# +# Note: For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See http://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by putting a % sign in front of the word or +# globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = NO + +# If one adds a struct or class to a group and this option is enabled, then also +# any nested class or struct is added to the same group. By default this option +# is disabled and one has to add nested compounds explicitly via \ingroup. +# The default value is: NO. + +GROUP_NESTED_COMPOUNDS = NO + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = NO + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL = NO + +# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = NO + +# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = NO + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO, +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. If set to YES, local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO, only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO, these classes will be included in the various overviews. This option +# has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# (class|struct|union) declarations. If set to NO, these declarations will be +# included in the documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO, these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file +# names in lower-case letters. If set to YES, upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. +# The default value is: system dependent. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES, the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = NO + +# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# append additional text to a page's title, such as Class Reference. If set to +# YES the compound reference will be hidden. +# The default value is: NO. + +HIDE_COMPOUND_REFERENCE= NO + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each +# grouped member an include statement to the documentation, telling the reader +# which file to include in order to use the member. +# The default value is: NO. + +SHOW_GROUPED_MEMB_INC = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. Note that +# this will also influence the order of the classes in the class list. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo +# list. This list is created by putting \todo commands in the documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test +# list. This list is created by putting \test commands in the documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if ... \endif and \cond +# ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES, the +# list will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. See also \cite for info how to create references. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = YES + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some parameters +# in a documented function, or documenting parameters that don't exist or using +# markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO, doxygen will only warn about wrong or incomplete +# parameter documentation, but not about the absence of documentation. +# The default value is: NO. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING +# Note: If this tag is empty the current directory is searched. + +INPUT = ./UserGuide.md + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: http://www.gnu.org/software/libiconv) for the list of +# possible encodings. +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# read by doxygen. +# +# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, +# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, +# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, +# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.f90, *.f, *.for, *.tcl, *.vhd, +# *.vhdl, *.ucf, *.qsf, *.as and *.js. + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = NO + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories use the pattern */test/* + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# +# +# where is the value of the INPUT_FILTER tag, and is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = ./UserGuide.md + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# function all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see http://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the config file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the +# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the +# cost of reduced performance. This can be particularly helpful with template +# rich C++ code for which doxygen's built-in parser lacks the necessary type +# information. +# Note: The availability of this option depends on whether or not doxygen was +# compiled with the --with-libclang option. +# The default value is: NO. + +CLANG_ASSISTED_PARSING = NO + +# If clang assisted parsing is enabled you can provide the compiler with command +# line options that you would normally use when invoking the compiler. Note that +# the include paths will already be set by doxygen for the files and directories +# specified with INPUT and INCLUDE_PATH. +# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES. + +CLANG_OPTIONS = + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in +# which the alphabetical index list will be split. +# Minimum value: 1, maximum value: 20, default value: 5. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all classes will +# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag +# can be used to specify a prefix (or a list of prefixes) that should be ignored +# while generating the index headers. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output +# The default value is: YES. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# cascading style sheets that are included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefore more robust against future updates. +# Doxygen will copy the style sheet files to the output directory. +# Note: The order of the extra style sheet files is of importance (e.g. the last +# style sheet in the list overrules the setting of the previous ones in the +# list). For an example see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the style sheet and background images according to +# this color. Hue is specified as an angle on a colorwheel, see +# http://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use grayscales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting this +# to YES can help to show when doxygen was last run and thus if the +# documentation is up to date. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_TIMESTAMP = NO + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = NO + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: http://developer.apple.com/tools/xcode/), introduced with +# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a +# Makefile in the HTML output directory. Running make will produce the docset in +# that directory and running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on +# Windows. +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler (hhc.exe). If non-empty, +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated +# (YES) or that it should be included in the master .chm file (NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated +# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it +# enables the Previous and Next buttons. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual- +# folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location of Qt's +# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the +# generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can +# further fine-tune the look of the index. As an example, the default style +# sheet generated by doxygen has an example that shows how to put an image at +# the root of the tree instead of the PROJECT_NAME. Since the tree basically has +# the same information as the tab index, you could consider setting +# DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = YES + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are not +# supported properly for IE 6.0, but are supported on all modern browsers. +# +# Note that when changing this option you need to delete any form_*.png files in +# the HTML output directory before the changes have effect. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# http://www.mathjax.org) which uses client side Javascript for the rendering +# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = NO + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. See the MathJax site (see: +# http://docs.mathjax.org/en/latest/output.html) for more details. +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility), NativeMML (i.e. MathML) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from http://www.mathjax.org before deployment. +# The default value is: http://cdn.mathjax.org/mathjax/latest. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use + S +# (what the is depends on the OS and browser, but it is typically +# , /

` and port:14556. (QGC also has a "Listening Port" setting that must be set to another unused port). + +1. jMAVSim can now be launched to complete the setup. At the root directory of the jMAVSim code, type the following command, replacing the IP address of the machine that is running PX4. The port 14550 is the default port of the simulator module in PX4. +``` +> java -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp :14550 +``` + +At this point, you should see the jMAVSim world visualization, and when you increase the throttle on your RC controller, the vehicle will take off. Note that the RC params in the example startup file may not be correct for your controller. + +Example "startup" file +--------------------- + +``` +uorb start +mavlink start -u 14556 +simulator start -s +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1310720 +param set CAL_ACC1_ID 1376256 +param set CAL_MAG0_ID 196608 +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +commander start +sensors start +ekf_att_pos_estimator start +mc_pos_control start +mc_att_control start +hil mode_pwm +param set MAV_TYPE 2 +param set RC1_MAX 2015 +param set RC1_MIN 996 +param set RC1_TRIM 1502 +param set RC1_REV -1 +param set RC2_MAX 2016 +param set RC2_MIN 995 +param set RC2_TRIM 1500 +param set RC3_MAX 2003 +param set RC3_MIN 992 +param set RC3_TRIM 992 +param set RC4_MAX 2011 +param set RC4_MIN 997 +param set RC4_TRIM 1504 +param set RC4_REV -1 +param set RC6_MAX 2016 +param set RC6_MIN 992 +param set RC6_TRIM 1504 +param set RC_CHAN_CNT 8 +param set RC_MAP_MODE_SW 5 +param set RC_MAP_POSCTL_SW 7 +param set RC_MAP_RETURN_SW 8 +param set MC_PITCHRATE_P 0.05 +param set MC_ROLLRATE_P 0.05 +mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +``` diff --git a/posix-configs/SITL/SITL_Diagram.png b/posix-configs/SITL/SITL_Diagram.png new file mode 100644 index 0000000000000000000000000000000000000000..bbd2dbcceee688c356722ca5a86930689fbf0da6 GIT binary patch literal 80757 zcmeEuWmuHo`mUgewg9u0wmf=VO#{=qI=>s%II>UYA}VNoAi>!;Rw z(ps5O#Gi;h)z?=^zIVph-QC4I82_CaIlu9-l&ABcaR?+`KV)q++Y9oOFC3+(CaAmfWNY%nW7@5`Zpr z_iuCkGlDk_N&vc;42$?bE(4PTf*|{|qQB1r8-oKv`tBXg-~a#9E!=0|FSC83FX;_$ z#UsE{_{(GfncV;DJj4K*1jMNa{Y57F&_%Alh!c|(Umwcx;9rVNNKnPeJL-pOA@wz@2AG1!OpNnuKv^w$Xr$o8Iu~_*qyc@w z{4NgNn8=_@2){LeS#kZYE~zb9Qt)i1hzkW_s<#ASZct^1LzAa~LGbAUJ?&e!kRu(T zR2le3k>4}ywj~Q-Af-W}R7j^;Bm|5~b_l!h9?~@i9_m98iPdXKir%QZj4g;oPwO6m z+t)^sugyUUDS_8~%z$~FdEa`Nw+`_6S5IFE(s)br0dG{{cj3G>_(1}F=pez;-tBBk zfG3C5IF`5TVg=gFmTR~u0h11V08Cm^?2rd|ZGd=Cck)q|ew`i&=9{7EGm!<%5YGW6 zATW;N02?WvMC7VA6M~2`k#olfCdD=+Q}`G;sW=QXIE^&T@3^F{pK^M9?k_z)3W7}9 z%J`5AV04Jwd zIvBkj_Z84YL-XS?(!m#}fpLrHJPQSp9sI>B9f3G=7KD@x;ttRpWM(u^i+o7{Smh*2 zeC%y=GvJ9t&7BfCq>crF7PA?TTu6|^qyv*#yeoR^sN;BmmI7!8M38TFBQ2r9M_mvp zSvFc=nzT6W7J!xrgM#nC#AJ@nH2%mOIX=t(jq#7##P)xY^)>l6nzCAM=F2pL zABpu^IAJ96>Yg65{E0&S^uXk_cUq^BlPC@L)`hB1HHS=1;E-niJ{Umg^Dqk|U;L|W zk3Ss50^MkE#d)ONT`$@241kNo1|j8!`8^6S9BFP6W@%(2PLxK3e!oWKW@5gXx6Z;5#KUAga5;YKs^SHUoKnqO0VpO(0iKVdAA4gdmNcg9@tvdC^^Lo<3-poU$%!>NIBa*1xzZ| zf=x9BDFlcPC4cvTCcz)evI{SfoPxU;Fp2|Pa)zvL_GJKZ1Q%#Tces>UsuvihtpGj50)b(Idh&Y@yK)uW` zqt98meeu`$<%X{oZ<)z36ngnsJSn`6wkaHRQgn^wGgBk(@^A{PMZiGxv0MV~6y_zJ zLldgO3HpQIBGSp>Giq#xJEOspi4e5;y-Bq9jg3XlBQ~B44sE{j(DtxUWhwXf%dLj= zbSUYXt=d7`tNt0Wwz)*HALkoKR1E*w?p9w!dpMrr@(iThb-{=h(`8w{yr8X)B}o?c z{JiL;_hMc;>3+0UrD*!;SN>mXF`?&=g1^#>MH3fU>!PbY0F~%Aw0shN4R0UGd9X1| zH2m5g2%oxFXNb_uM%`xjVAF7LK(%$itc!e^d9NhQKqfg{$<5htqQ{uYUDaZGc+Gl^ zjf8_2qQ2ck1}>!UKD@Ojm2~D$R}lUkLH^^L#KSbUS1^Dl7er>78>-~*uVlG)445Z zyrwYItkeC`%knP_W+Z3cOrMO-3-XoT*pBroXIkJ87Ro=B(XfP2ZJWxpS5@RFREroI zvH!9?UGE@b&m{q0RhYEwNXU)a7arKsb^08tIfgv@XUn(0>FJ5vh+ghoyb0Zw{Kmv> zQN5iR@buLOh}+czFS+E@fE}ZLJv^d>=QWo&^L367z24ps9I)1YNe-;BozI<*oGhbT za8ld5PdRi*-3Z|n?_!pvY}^t#1QgD{<#fp+Q{wXlk*c$}D7rc&R#Fg5huPnR{^A=fl`j_+8<#$5; z{S#NiCu^5+?hDJ?X)|VzonK_ftTms*PkX;`$!L(FIyi2d%<_2=%TWo5T9r&9q!RCK zI;@B{m9NqrDYKj%n~p^eeLp3EULa_~tfu_jCQ9ool2?i?{6sIpp8ltFVVvYc^M(I zP3}^C@evtgHNt-1v<}ZFpL_AJp@Dt9jy0di?>2++8{LjZ7qZx@rZpj! zeQZU2hyH4}iW%y@mrTB;T%prBxPNA!Mo;IZ(#)({!Gmi%?Oli#LT#QO1wGk#)(@V| zVX!h~UkWd$daio>qTHCyMwhQdH)b{A1iSVyFXd9LsMwW+`1cKS@@#Y9Qq`9Jo?bx& zr^kJ0duTz&e($}d*nyOS?t0D=F;_K3ldjAr3@^uZp3ECglsawSjxNnoyIfy>IJ|Fr zLY>!xb630eGjyMT+Irk7h=4u$n>*V?gjdM>9_-(Z8`gT&q^6)>V(^|ZJF81#qEdW@ zx1X+zdkb0~&h4+A4s6+qwc_yf~TPABQU^Z;;wRX04&iplDsJ-|2dm ztS~|7RbR82(Qv)NzS#O2)hHHddZ!cEay68452Y6c~sc3tzJwC3x&`Vc^p%I-pz?j6J`SN86puc4bN@Z!?yKPS(0( z$nGsrq=gplyPNSoc3@h=dGsJY*E%z{m%}fC_&GWF@fz#q1X(l7oc9jnK&QGdoTkGM zf`Z&}bC5f3{+iu&&(AZs7nJ0PI6A>A=(CGxQ#bGFclRYep8lk~ras%*96i4=*A

35|u#2;t=pUN;ye$-)vvxKw!tuP)#;EAE@+>+XoitObQ^|1<}>D zqKId5y2pmBkS_de{I8#XIt=#Ln9s9tUol;Yo9~>yZu7m@JOXS61K*;z4B?E=wU*C0 ztOKh?7?(S0E56`XaGusWoF2S}`SNw`4)KB^ZE@UMhCka7c!r|Mx0#v}kg4&hIDG$L zZ1isT2xTERA$LCe?yzyM%@9sE9!9H3-y?p9f&uY`Lp*%&r)E8L7wKpmrO;l+aKq)q_z2sxgp+Hg;TJtMX9w1v z*$wCNc~gP;pN9~3tD{m^TyAyb_M_h?NWP@}Uhp6}h+^@@uez&D#tM;)i;y?yFEbjo zgEYBG9NYWt8Gkthwnm$)Gs&-zSAI&;B@sU_BZvxDBKWB|SEL0BkKN;Rp?eDj@7+6j zR@gMi=`AO?*FOjr7xKf?^s^n!bc=I2r#;qa!4Qv!o5{GZT^SxjB)i!3m{#1k_ViCt zj#K8oky=)~bFohqZ~E8~HVD$L3QK}Qj}u?)Tm zKg^H6!c{Z}F}!dvcsPI4=$s_x&etVfR1t&iiP-%WQI3Beq#Tc&Mr5gGY;6wCJeIAG z^J}8KX^&u#9)B=V({cDvL%^sji9|&+jwI%1+k4-(H9vH%!fHZ-w&KvcZTCW-VX5l> zYKxd>b-=~IR|z3`ef=p5=J(K{zx6hehv)!dVUde!7{oOia!Od%d*iC;Dt9o6ZHrvMvuj}OLwK~1y$!f;2k4&CCJXe0+r*B4=Xh1sT8G8 zH?U?81U{|gF_?Bd`!+^p#>=0^4{zmd;`OvrK<6LM{!zcEFB!4H<%A)t(;S-Y#FQpE zestzOtvKs)rL5K&5A!_|cNhG?bWvqZ{vuEFG~mxsIS>m_;@MsOkeS$PGF7=VG3y>; zc29TieSXU{TFJ%j0=CnWK8h+i82s(;#r6?K3)z=NCrM(bx)CZaEA2YEMzE4eI%yvp z4G#43!~#7aCYS4v!t5ui)1O!{mJLGWa6ShD-PA#Sh z!9^g=nEOjB0nQQ5mZ8ou{S9qosm;bXif_L6sU2QzCi$+838 zWnJlY8=+}!{_Yi2M<+$o*5^|hfff>Gn`4r3d{P^oN4h6;TGmnwXKYjgib-Mt(@;i) z!Xb-MvCsT@rcF7l5BD{}Qo zZ-O=tY1Si6&2|TUEJ*Ba@N<5fS^=l~`3&wgo0;%2^f0>OSdtj`si)5&UD5d@J^m-r z(rK$f=5-_=))3T!Aa0=@kcPkcMAXB&)3#d#@yQ(k?0#Ar@7*BYlWiCqMx`SCsi(t& zb5ycPL@u5tV;W_OU1D^*xVJRhaE533qP7BhbM}S&8=U}C@0!Cf-9sU}jmBRRRZ*6U zCq;bKhRnLnP!u5jAG>*QVBI`_es32@V}XsSzD{oQ=@(sssY@E2YDx>k?}&vuAjqy< z;m55cTwzIo3UJ^?AT+Z6=r(rWi)_ILJwOZ%gX};8J55x-xqb~h%9PXrB=WRit^6}Z z(fBF5>D17FF6d2jY=+v1BZaxC5EzSA`{g$%fASW&CghjpqIhVM{dBlG)GCA%s1@ z0#W)bmwjmn99Up8FY^{^mWuT^-I<=f;h0k)Xdp^#nduBMk74bB8B7B`Eg9 zth=K;3F`x`?JXHkG#iNzVha{Woty%C;t9anPPYE9>TwOv=QF+p1QM@)N_S10-QX~Z z*)8ICgPL7lb5y}8VZJs><+sobeM>UB&;!EMm$@x{AtZ>?C=B38=|7zS9QR|DwjgzG zB)zXe5)Jo+%qA=3{B)n>6ju5&1KH_@Vg^e-VOQ`o%WD0-sb2NL8ej5iM*w0_W{$(! z$&dOmEGWU@0%M!2w?9Fo_PH(2gX-RP-)a~ze&iL(Lo^=txR8*}u++0lr-&M5jhx5R z9F{MNlZAqTbByQ-?W?5nuzuC2e;~7ckBaH8SemyR7SgsKKPt2PQTnLaEY{Z+>>u=L z&CNIhgY2k2@+7gGqcFS`9GD4M8ORzs&x4kZA3>C;`k5j$b|9HHwKoSifJZACB+WO2 z1Y+4=12}|2)+^?m`~)twjzk3YDS&%jLe5P4yjOzp^R*(gG(gGCA9ZrrX(t!^>I|Wi z1kg9Ekt_mwhqqvmIgl_Vk1sXoVUprSU<{N~`O4VD&Xp}+o{^qK?S#xW*^_z6#pj;j zx-=V-RACZ<3{lqt&<#z;*jK}D(fX~9yX&D7==vB)l+}g-0RBvHdS=p)8%n%)Ej-P! z|0nchi#trau2_~)Ebdz@^CxkPhO*-`e(tpCu2zJ;c(Cy;oEZkdncI~_mT!=cHAxT;t#ud0JHe4csbZK^`M| z@BPF8(Y<)y=C9?vf0^I6^^UcdL@biie}9O|VXPrAOhvjNIcQ!fcH=){p1v%7FQEIH zlzd()I=?19;2;oF4p-k7ZI2g1Ob3F5j`P#cZIv9EVTe@eAGao=jXcj~eG7@cMa$#8 zvu^h#lT(Y3eajldv744b1j5*N%j~N6@qfnmA`GNWZDuJv&;T#YO!}bHzudviINJ^jLSr@h7~sIK7(Ua+lBGTXB`1 z*m>3F*ThqcEB6LP3kkc6Mz7!A78oI6(R?)fbhWu2D8&&j0PHUBi7|3m91}Z9R9zpY zNr!UAHJOK}5ye8dA(gp$7^brRN26wew&_2*z6p{{dmWMWs1-lFI4p=|*I%M)HaIkBix(C-fq4oHsFnwjI-L(91YIgPsmURi!H7@9RFz4Fj=yvPIz?M@BY z2~<#{QL+Lau|d3ejKq#7MF1$AdhAOw+7$Kf?sASrdzN!6%}iTWp0X&~r>7Jj$flq4 zPr+bS0;hI6Cpu?YZ%~?}x^b%fzIcEuV~^g^_xd-H2?s^=bKP zN@-&-ohMny<@4#+td}Q=E_UlDZ?z#__k}PB-YpeIcf|`d+cbVo?wmz%c#S{CzAn@2 z>iuBHF>WKAww3h=z<>r`=g19Z>^Tnwpf}oULM6X2aepFYeFSe3@EoSd8_68O0VVJh ziY(YzM?86tubc<=KEuG*-|nbl8g0<4dFFYw8ZP;EhS!Tq0<;5b;}-bBFa1GfZD*z? zcQJ*`vW7~HCQmA<{OqE0L4LA16H^<%K`~16rhVi&c%Kmx3# zWk9rP)f{_&g3#~w_(lM3^MVN*wXCc2nKDf~pJUItdW3?L5)3=XadI^)ie1w}N!X2+ zVQ3YM%j;B4;@7XKmS%kD91F~kAV7e_;PH{bP4$S?jpO?em!Hw#9tZ+yl&}6cE{H8N z+L`WzBmT|>03XvnQW@~olg?-YD$54W>SDXFS1Eu*b;+nJ+E-6o|_l- z(=|T1dvPkhRyegkMRJ!m@$#gIB3Gx3+J5z^w}$i$#`X2oNawyrY>ayCKAvu)Yw)L6 z6^kizrYb=fsjly5>s%i7^8Qkz5kc*#FqKm%R&*MhQOHrGiDJ`3%V78!a-`dIeML~H zQHJLwqf_Xb0F`ecLPB9>;w6VpH{MwU;Ddz)%)Ci25>km@w`M$$1aQ(4^bvjK2mLFjY|2<6{lqnUN7j6!VVd&TN zp+7XuwH-1}e-E+UlU#>vM2Fb)`|_x@NDjbYfu}iW%gMmf6;_mz9 z4p{L{0>ZqL;epv8IK7edNV}C}U@IilSEh$&8>^ZqI zlVPqCu!q9s)Kl%AVP82xhR==tWbAHIUt=88HhE4w6`KevjNo4%YV&ocuzBGO-&zg% z?tJ2rKVy7R<1pS@klH+422kz#gusaY`R)--<0 z5G(&@rZvw{=O%V^Y^Xa7S1Y#SO5LNu`8|+iQJl;BLJO-pQiwQ--CM zZn=EN(Nb|jej5ONEBQg|{fXis7er%zGwoh`sy^_(gb5(fy}Hi%oC6XTmkJn)V$qVq z`4YU#u?~41$SMUAJ9H(YYqWx=XHg42fWv&R+$URZ;sgv)13t(3d8RmhE}j1B&Q@nX zPF_u@(Qa%bdTCNM%8?l#GF|{i9ielFPBxpOUpQL(=g6hxW+@w|UmmUx;0*`}^+c6? zlymdS2;lDir0x(K_^X5MlkJz3E z08dOUAC3nC>54ZF5F;olz$;tR?tuY))2Y9`^;G;<(A=)|dKL0Ct;gtS3|GgbH;#nI zBB(XU9ov4c*2(z|9g^aJGQ6%IDlvXmX2vwWa2}({`>}>&NiFuVvefW}m6WKD%$s7h zqPUBn-7PSMfWX`y4LV2{?-f;5abQJGnBhg+aRgk}UQ$kcyJJkH5zt$<>s^`fK?7+$ zkm%uf(%8y|Q$cM0OAg0T!Y&I9K_Lqt7v}2R4`0FlTgLXw#{in&xLK+>wK*ZOpx=MA zpYP#8K;JehERp^?>Au-SZ6Nvhk%2E${<;WOG-Y+>rCEhH0ia+$gN)KQNWh~Ixig<3 zs9_(C;=ZyB@f*Tl9QU1uTB;3Jo4>7U_N`Vfh>C=Z9$25Ak@ZFh*ow?`wr;q5_Cud) zPSveS&$lV3@;NzRjV66|B=9p4qRQIMBwY~z0x6ozcBPE<$k_WWtcz^B1m!2rD;5#u zk1E5`jZ)Cr+1Zuz9+uT?gxI1RjzybTdtS;i=sCJ?FUB8%QPsimkHX z76jP|mA|~%f1g%d>>mYZyg%+iBf4*NJ`o?;p}+JiI&Ft0vfS9dx{#J$tabc%l?*FZ z{aMKz^yDaS%i8tw1cynvI5>f~q+n68tJzO8Uk%s#skUs~xW^BTmS8~ZiMh|ziI6)( zHIf)53`uN&qWR|ZE5TG`!UcV34hi0@^b)}{ucvul?fxXg86N7r3BfxUcxv(AGKrR&0zS7yO*qDGBY0uJQi1!JJUm&AXBv*x=sF zZ?qRYCC&Sq9Bjm$elhk54)=8%0vxSp(;9yw-$SiS7laQ^e_=E=TDi%o4^MomUTs$W zP3vIkoQvC5Of?K&4FeAjvd&Y*u;e1Km2Zy0fn#3ADiU~j-<~;) z%2%-z9(Aq!jF#)8_;M0$cpBD6%4;8_>GJgPuY0NM>k9=^hxS){A;s#A?kKSq+!qlQ ziAq3H%=e0`6d!3HR?Gn7%@Z0S*X6fmdlP4p``+^+vj{FV5g^+o`hjGSCm3sC`CZ7T zF8q4#-nCVR_th<{{MJlBBPHGBM5JRm0Ntq*gWJ#_R1KUo*IU!6vY2_`S7YODQS32> zr@KeIa6Fe`ebqy^hB}Nz-H6hs>7*$5pc+ElmlV*taH?`C&{awgCbT00yj9Ko>3Ag2 z=}T-tZ!93tL)0J5a1n}rquxb4UxsxFR=fzQG3#&2o$--A7R^8e6l2iRx`f{RA|oGV zNn=pxEuM8{w$Qk|fKZ{m16VS61cIKol!y>ul0R4=ejvQ*DN;)96_EEBa3&}O4s!b3 zZ<3LWI`2ckZGJ$IuVCZ$Up7>j0N?EZ)37q_6qj11el>u!em@Qn2i#fi^W(s`G>ZPl*QD_rSx04R3E>qg|AN!>5znzJIu6 zyHd>Z*6Ug67yaRiOe^g)fmS)z|YbbvM!)&k?VRv-;X@4Hb&*)VdhQQp#i=+(EaF~6sHbN%9IG#bHWnQ0(6W{!F7 z`^I<{6z_~APqy>`GgQ65x`{j+%eTaNbNL1SxekclnC81P_idD0^?l6kPSuJk`v?sA zt^?ivBO7#JbOGu^z=*xIkh~uihXa#8y-)&A94ggs^&UCkqJQ^qyh^L5fCQy=k`sXS zN}!xTz8e-CB>!ph4=hqVK+Y|JKDSbar~)Tk!-QmBS%4Aq1ZYh|DU2krd7~Dy12Vs` z?h-)i8tFoMwz{FWZ4!}W&R}w16#yL276m@je+!OAoyw!1`u!P30qB5C%ijqQP9n0ic&$LE zjN4%nfX*hPVi|J)I-xx91cxHJ<_}Y~15n;){xZNcUZTA926(8b$WNHiLSW2J9zk0` zlcmTOqI{lz%uHKv@vjC`>?PK47byT9Vt}3Yj6)8|VFL^Z5ChPYGe94-i;Y?F_^+=3 zt#$x9=DEMNi8TGcKUic#s`G$*dx@>T0d)KI14cYP+XmR%7^`U?;{@k)&-P(6`Ml4t zZP@K&yAOa`O>Jq$Z(S+@lTQq~Xd!KLVYgomjGTb+ha8#FcL)0J_Wjb5mWVHrqiAH{ zU)%i!x9r3KV42*6F1WYPIamO3#(hG&b;u;($p|+F^Q}!bBH4-c9xl#ER{(JG@6MM8 zyGS|l{pGb%pp5|&@gLP%B7>mVmvy8Z7D-WRFhPemq`oQ@*%C}-Grd?C1oWcWiM8_8 zuUVst6|l+-J8@avcG}R~C-PWzn_c~j7k&pRj#-FL(FQv2w3K+bwF1G8`fIXTL?Ml4T_@^^^O{9)$iy3v5ly(5v4TuQ8z94UVmJiA`R;_`NgvganuuEBL5 z)!BBw1<3cu1`cDN1nY_g(AYFU(^kjXb-g#6aeQr;evtBc@XSkp;Y8g$%#qty*cn=9bs%$nUkVBwbV+) z!%+PZ#~Z@bg5(O1PHpByfAg>+QE!OL8DN3FH>J=5U>`uzPTgN1h5xL}I?1n_vj>cwUB(|xBI(Ei4aN9FbR#A2@Mq7%{}erUJQ!C*U< z6N5Dxl21tatcf{|dcm(16Gj%3s8Kw>ao)`vF>R?}lpAbz!h(EvLr-eDxKFe-8mj7b z*s~h<+=!ZKyeRzJ+iz)J2{G89XI{E4_W@53z0J12!z%qjgvdYwE%cTMK#%`DSnEnZ zwy_YF5JY*7B@Vrl4WbG-4k4zN@cDS=dc-F~$%!{JUMO1jDSF6oF~W>0?y|{&&AZbt zFtx^ZOtC0Il_gLn4%hP(H|2)o0e!x5=z%%({Hk^jM#l@f{)4VHAP$Fv8`1{ww z;>QH}=mT*f_b>=JAqZO_9@}98vjlcFD4qs@+_tQu&2mCEsy^LA(MYc{8Ce@5PU1V0 zu+pefEXsFWzi{F%b8Y#N7ev5olbJ#>UPBW0YuX1PNchWT(DLC;G7ucA0Iuyy+r;iH ztZK#?9?OJwrN~g@#D%y*I-s_03oFbQ;6F(w&=|&b1QQ8VPaC{&=%0O}R*$O~I3v#H zF1GHbYhWB5Ti-&kKU*BxDt8lHyLD2Ino&_K_0v6wPv_N`;Z=`JV$< zxxK%W5ci)f9sojOTkvbR4WhC%i!pGT{I^S=KlLK$rncedBaCP7&Yr9=t5Qy|>$cQ9 znqCBODU0K*L^Lda{<$2dg}`4A6-t%Kpq;07;$OS#)>*aT-T78lBfh0NqO8aSv4F7gsfEU{Y-sXLIIdDnlD#~Zo{ih~^2MOp#4G_nN!${zFfI=iK`jphN6!HSh3}k14^WoC29j3H@vl*N%$&AT$F8;v4duus{mg=%1Y#S$Tqn)D##1GJ!@Y z;rzB=fc|`eMvo#~Zf{h=fNTOt9geuVwgH78Rk{e?>8eLA{`SHi5VZK;1>H*i336LP z#7F{=#@j1QpI_phBsUgb#$yG7aygOdIcvcfz1=)?ZeXUaB@T_5;{*JSqcx?s&D*W% z3www#GF6Es0meILSiwQAZ4lWL^QH5Uu+(>a6yGDgn>DN%QQql=lfJG`}-{o~!)&)bkG&AO}eW zxFBe`1k?`^{Xb%{f9MB;zfXU)A-KJ!{OgK=t5n6Bn6vFg43^3F^4%=wVH}h%gzMZ-L<0DRxGu#c4CjAhmAQbtyu!6V_qei)QtJW zKmBhiRR8n67VMX<^)uj-vryRnjgFP(sVZrs30CD>xP6wie~RY))b7&m|4hdp%+V!6 zUIa@~e0b-usNp`7N$kGd3G+Vpf}a;mY{+L3GOZ5y@X(q5=Xr?r{AePb{dlHYc5D*{a)m)6&NV3R)RjD z(XiHBq%c=a=3)pKeihnqj`XA1{P*57S9^iFP=JPyQbQ%E&|B8i97>7auQ$ZH_6wdi zL?XPjl2s6Q&^-U-3w!MDbt&M)FHz(Ge?uHGdY=LZ263iEbGU)=Z#MPnQ-HTWgK}Q; zg-(AEI`^)h5)2l03mbmqn&D^)A*x8X_2q+NP!oBW20M;9a3AKO@ zV!!MBBMX9(R8{=gJtJs+$cKWw<=>=D72Cii(Sk`WmN7oQ@B9Ug9w1JalX?+SmUhL(G( zv;wxk8fn;`d0(D-yx1tJHoBPl7(blmQk%XdQ9q#l!xy*_pBrhF9Pk4WJ?&!myokf@ zGV4E_!_m-#raB(n2Z!|l_N!?{P4!$bMJ5W&X)Oehg?bk8=u|x809lCxvXZlMn&$#y zah7*Ro3EdrA!9$SuO2z!&jhZYAt%*y=&Uwn_VgjC+EQK>ZN99kTAZi%+e$p6O5)3i zQq${j)s#4E(A0qh>d(IB&WSX%xCfI0kOt^ErnIVc%c>=>GON8&U(jomF)>TR;X}KT zC7!Pw?`#Hq3#*S9HXeQan7$x#v9Y>ccYIN@-n#G@&)+W%(TJ7Jh_%2>!%U~)nkgMm ziNh3KcC>tjEK)-b0352{-%76q!C`!YKU?QUHNU@<2L0mRmBNVrl|k0%nCF@4NmKiia>KE7lG6T3=dC)?oz`oTqz)fFh`yYWg&RYes76 z>X`m$wAyP$x>rknaaG1C(;n088u(Ry+mR$C*kjTEv#isO43J{1Ig@G>u=53a3&LQ9 zuL?#ZG6S6QX-unScp3!(CV;KG?bT#Du6^d)VCUA>2F%O@@$gG{#D77cM;kzGH{1%QebCz%F^-J?A~RT8q0sW+0y@b~m5F_PkU zHhU*&aGF=?ex#%MY_i(5{%~gAEgtlLSxi(~<=qHS6rygf2jaIrTpVwhZO?F8R%LAp zY8viVU#oj8@-4Z#7<9)biNbu9D_zsH&Gc#sHN)bvwQT<-T|wUx#mzrZWxINDBbe)< zs#^Kjb^OR-X`!C!QA!mmKo`O}uaeFiK}qwd(`-}D!O zusfXHRF}fPyif8ofjNeLV&sFn~PqGh&Bwn`o0tk%7BdW40 zBe`v2lqHiv{^}9?L(ljjdF!prAFQf;Z3pX}E!_D*od7KM^6md7*=7bPN&lz1w#1zB zYR}r5fv>;YZL@jmRcUa`^&YeX1%C$aN1fW-dufJa!}*Nqr=#?B zeVu<*J;pcbCU~Ung{M~8LsQ#Xk(nO)h)#B#hUNCw(97^Lqf-MbK38&##^p4X_N6D$ zTwL?g8@_=9dio<}TybSCB^TqMT!NA2`Y#pjE9Gk zp9nr}{*~eN^02=Ghr^3Z$&~_EA~3`)jEMy@ zz8+p)TS04#uXhivQEBJNj?>aKYsIdhX`S*}s<_BXZyd3d`_l5zVeF*a8or6`T;L_c zjZ?^KAj`zlfw>gRnhd8b{3r2rk*>R<0p;4TqqlCO!%5klv-3Mqa-GjAzVNGXK4;P) zjaOpuja9QcW+~x^QWl(`e^W7aBQb$`YkqS=ELHK@-Q+L*uF|;+aX+CHW;@fmJ_YKD z(&ZZS&2DB0=hptIK=fYU@#ZCwe1RmGTcg^-Cvo!q7AjE7nXB7mLD-+6DtK-VU)ajh z(mnZ*(iHSq2xHXrD}#)8J3tOR5dK|M!^S?=TxA&~^K?Opef^+~O`0iNI_6&Oa2BQu znfeWg2P}|rqmyB0bCj#|ki>SOGlzggoQX5rf#6tC(7pZjv8Xw~YuL5DIq{8%U5cEr zUG8ePlq3}fzghqJ(aKyg_ww4Y4YlE#_ueZfPnV%s5;C&aGz*`K1qy|&XzLhm4wu(@ zjHj~&BS_YoLCNy$o#kzxfy5QMe-Wzr^`PEl&+sz%kk4zIZ8!%iH`N*Zh~n;AiNf}x zWgzMGaJGS$<@L#}<)2dt6>H#vQW{@wFXR-g5@$&m zu9;eolJDwt^o1fgztOb2oh@f%Lyy;3g(#oBMSBP&44c<)wVMf;Paa_vg*On&ZJnfD z5YHaHf|MU#c7zA&O}pR83$IdTAiq>l8Y`eqlPNTk44}lHTyeH%EsM4Nokj9eAd}>^ ztKrJ3?zD_Rla)Nq>Y)*=-^;7GPd{Qla6mXW-?Qd=#RXfpfo&Qg&*rRpWT~zrNHAP+ zaJb}T5Ys_9Sjh9qq!%z(5XE$^H0syc@Whpw+5+X&(cKEMF#y0Xr)Hb04Ywf>ZOd3= z|2_ZV01^;4s^)3pnsWE;u9tlc=nGRY`~3Yzu@rM@yWO?9!OJL)E4RSc5&YmeMz$aR zGpC7*CG78rNWr23tVy-+DGVp9UTuG(AN($3;7BTn<(vXh(%)f6d81(Y^ zxzs){S1$PA#rY%akjjC!rCYjucKxW&jk=K_o4sWE+?e*HB?bi`#TLOyOz2pjXFDjZ z($}xPDSakbIY>uSI5Ih9;K>MUF!~jA-7yb>XJD)f+xY64p4uR-sA z1!>*$6^&r1z>qQYslo0A(MN7M{eTZKNA8DXlr(I*)J zMPwT9gMrJa6#MaDBto(Z?8q@Q8at8Gy~hD`Ij|#^F0@J#S&aMmcx_bEq=(@heeK6j z0!q3{@18fSA2?k!FUJxv29*R1_+c0Lkjt^N7m(P@iJeMe!L&`TI{VLg!E|01B`{4I_f?{*+|_`>450xXR{EaWu_ zINjpZ-laTU&v%Q@V;L6+ABEB!`G$r@Qb~}Xc2yffndcHtUiP;5qV%6Vd0d|L&o^Yq z0>{rG%j(4vUq)JOQPJG*J@V|E=Jp*xxnHKrs#l!UY~sd}#w8ouoH*p~#Hg=;?K%eH;1GF1q~PNIs%QuQVc^Uq2aJ>|xGy}hNI zG>t?DQwa9cuLG!`R@EG6jk+QmjDSgyD*L8{`Ui6H?xw0ehQGveI~ZnT-*fG35ZW0- zRF;?&w+!{th{dD}Jok#Hpc#-d>ii5Y8q>bNSv2XuW0adj43_cD-ZX80wbqi@=%R*0 z-~dRewXUcQp|2yCjJ!ql&H%39DCbbUzxh(bVrVIP()4OHTau7Ug(&@@#~zhLy?>p$-5qA^S`UZp-?Qy6!jz?BA$G3n8@ znDFq8Hs7Bm5iSqyyRO}ZZL7Zhuq+Y-YHp;fH!+p%J%OS}OcBFzD;$Szfa+IW9)-R9 zr52U3WWFsjmYrsXx9f+YMr&6h0&lFhz7{Uxit(7*Q^@0~toE2*nv|v96m{Y6LZUDB zmdq;MiJXuBu13^y|2~EkV(P4o992wfH>O?}F%Swt zx(1B$-kt|&Gd$?E%Uzh}#jd~%Z-n_ac=v{UpAF~9({g>#l+-vp2(@fns{#&aC$ITD zj~SZcnpoPhs$4L`9)R#I9?y2!CuA4sm`6u>yfg}m{@ICV$qE?tthJ`+5J}RggV75k zS=)ZGnK$&v-`x=B{{5Fe&&`qS{8a)Q5MphG!ei(P~{-9D4SDC>r+>6kE z`xaNLyqIC&_7`;affyTRd$*w9CBimh{d6alhvU+7o}7C{Lcr*}9mGu7IpZv1VaTCT zuh+4f0G=Ajl(%f?)g{=Rm&R^hks9(G{mD@;EbSuUhhAI!6NT)|;0^#pj@4Wq*guMd zeFHc*C-fAp-CrxGhiiw4lL}%*I-O!FK!a zb|(n37kKo0?npYI(@NL{Wk9b{6!?7-6`;(cQ{`Y?x&BSB>rK6^o-#q^s=u1R3cZpp z&+03#e2;sSMBvOY&Asj-;AbIB49f+|vjVwczM~=(;K$O5yZjDPtj3y;EnOQtFNryW zp=R3`7Tu~Cf&i$de5nbzS5!|t*}`*N7m*U)vw{CX*INff)qZQkf*`3wH$zHy$IvMy z9g+eP(mhBD(k&?6-HmjYG=ek&LxTtd5|ZB@pWk!NdC&WP{|(H{-s@g_-7Btj-D|km zcoI4lG9%xXB-l@RUcz}BX|>Y|B4!5z)+P){MN(hcnPOyPbaW)}L{z^cvi#<^!M6V$ zl5#?R_;sM+gv;(woR9M`u8+8Sb1+z;j96LsQKCs~3B!cpQs~@0a1h8t?*HC#Pd21F z_Z996}9PW`~3;J9f?5=$FNliEO2JlfTUB(k@pSUu}|5c^Np@3BbWR zV(4|J+Nnkc@w6_|z`PfER15>*QTG|?nGmH;7aFrxB;#-LGx>)RdG zm@Thi@*8MP6&J8*F>e7x9*1ZSIlE(>ToX7;zc1;{HPLCzLL=2=z#lS(10)p3a_;}V zy%D387=f;#5I(D4>ugUY8Jg_M_9VvgsN$#4=!}BMH_Tg31zF!0G9Yu%olqAYgZ<=A~l}R zg|)Ohxcirj(p(5QR6~LC-4*TaTjALc7H)A*71S(%I%3Cv%wojn9rWtUE)we5 zPD9B!?^PiN0G_$75>be{$Y+i&i-Hfd%L6kOc?{egl_mO5%t|RFfCe3)#3I09_=5j~ zyT0xHU06O}d#p}9kpWutDSZ1!lgWfWdlkSBf69+z{q^g1KsNHl(c}k!rJ7X*R`v$# zf=GKCULSItn;P=@t~t8Vk$}hi@cyE@k5Fj=$aLN9FSh+m3{*7yDo`hkYdj`{b(3qgb~fON#WwoIX~sJeT#%GAf||d_nlNR7 z`AXf^fBVOoKHx3aSI08<5Mdnkj-c^npi8;a-~P9=5C^nxPr|KsQ~|VB9`)z;(6#R@n*4n<4H zKLe5vL>COWJY8C%WuO2%PZ%jJ_#KSM&CYX0_n z=qPQ;ci#2_A~bY1@CtD&XAj_GdQMVv*#PKU_8-vKd3#Bm`3(!5~LGl8DV&8hqcvHTL->7-6(D0I^TWSPE zjyMG@)Q+K#P(~^M#o4csVS}0kcL))7vY>=$xm2D&O@Md({%!t9vR!t_46Ty(1#kZJ zeuK~`H_+AyFQt|M8avH}fnvRVm7dqusw zvM-ptiP#CK6`bjid^o-zsMuqJCT7Vhun;nx0dkC;ncE^Rt&6a#XEVA!DNtI(-=2uU zb@+Uo2<(u${htOr3qscBG+@(|H-0}`R`K=RDHM%M$ii2FGcMB?hOcVrL%VZG@t>5gRmk# zzDw_#Sh`7o1g73?fC8a~0K>1a)spFwRlB6zKRz3dEt;n>s4 z4pbGN8T*0_jMxB;3n6wk?m;^PpyCGP3vAFAfXY*@SDnBk$3q`}?4X4Y ze*Pj0I5<#~3Ou868*hMA*iDGocg@=61Imr9hssTDKayGRUm_g>*Odhr3*8ETT@^5n zFW7?$?U^CZXyQf>8wU}<^pEf*jPI-p^NlE1W) z8BD-L*&7o5c{0PRv|2)E1j8|g~U1+*_1hgc=^FmJZ-VJ;c@NnJ*_+uk-zB9#G zjz5}1Z!Ul3cy=(fZoJ<9*m8AW{%d4~53x#oF$d%Iaf zMuyqiucN)WT-;M4rXcz8<<7bL}ZvMC(&S*+mBV(XSY3^W~5BNmD!A>0kp`20C`* zks((W{btxyf>?{LTXR)FeKymDj3BuG58g|+0}5aOC=RDTpEvtmikE%n41vj=E{;uS z_Vw)q+)Mva;>Qv7v`^w#FD3^g4yk1bd7}GWgJdr!#(O`6b?2i^H@$d@O(~43^A<&R z5iCENDdr~~66U}5RBn^#=zMAZ?Z)fnT72|&X9Hu-fX^yS=@NW7d*9!80p|Sg|an`lm=LVJxXx z0)rbHr$?fD;;zU|RHC>=oo~)|drH8W3EVnL=@3&0Ghn~M2yZnr*RBO@lE6TqveUNR zZe)1O|)w#42@k4t`qpqVp5tDNuT^+J{E_LYnFf4E~HDubUz0RznD8cVI*y z5ZY`5;{!%1!h^hBY4c6V&W{Ts@CTY>NWg&!eNW5=U@iI$R*ZOzFUV}6`mN3kh+~d* zR+Aw_4&u^feEZI{CtOx46fw{r@LIETPkPlfZ28fr>0hkPED6esf$kV5`m^7;UzXnC$_{e_Pw#x$z}_=9lx`Q9vKw}xgMPsMuui7 z{E~_VI8LwJe`%t+qTTF{fLmn~_Fv=yXa29ZxEma`pFZ{nm+NYNq^rvcv!2X?P50d> zrm{SR4aN|^J1|}rfX%3>PB&?JxgXz1kgyBFc8|H-Mi$WBz|W`_f@9eab2b?};4dbt zXFZ-}h>*7*?tN7}W@RZ^N&QAQ-6e)$tZ*NJCFm5wOeUyn-yd~EcwMi3{9NkhmCn;P z;?cy%&ri28*}E7|2re>(EUwNruugq{^X_hJoZv!3M3)bs`T3-^dZ53r3AQ*(S<3TCFYa|*V3KB z^9==TeZkM)J`vaYPs;|VlMrMeffI}y!hJ$@QXQ}R2WSc8g!hg8k5(NbX zQ7gBEF1J5*T5FeD>?Kw2bXCPB7aMmLt{gJo`q09pb>HQ5`+REH%AK&hGmeIiq3oJM z$XfuqJB@D@5(W?;*5UQ>Q#IRLwh-?>v|}A|d)|s^C`WH~xW_ow>_d;e7kq9THYUS3 zhtD#R@1iBdeNAWsOsZB}(Ecn-cHd!*&{h;(t-{Q644;7+F9^WWJ%Qt{z=AoE28` z2UzhfKq5m>Mu1LIFMwqbsJ9E5?5&kNIW2e@3<`VMXNYzGWo&L^<4?V{e%m`2o8V{n z_ikIPdvWg`98VaKy1SsMisX}gV|Gi>2CTdD+QYMtd! z!=Hfi4OJj^hUl?n1tZ=0erYs+*)g2TYrP}r2Hgqxl@j^=?dG2u zoC*_h5VKa{;8LYRJXuAs#;pYs1KC)0ds87$j6k zb<)_WZASXE!^qrC%7^+#^ig5@19llPhguLmc0Fu%mZWGMcs!F7^6l+8RzO|8p?t*A zxS#zOr;&QgQrK(1HGu`UeTL6qNF|S7?8QF!Vp}H;p;tDLmy@mf4mClcWTzzOWw%cU zV;6|OT&w0m9A0HYb8e1TF?#ca6P=Gvz1#f>BO5$P)l1(}Ub!E}Z8b3y#7#AUS8J?? ztgblHLeMT9`vw_EM_P5|zI67-zI&#HtBlz5gK^~6)#)8t6$E>**N+kYYlb*! zB=hN0qe$v7T{+6)cRzUbdcQM+R+I^4 z54Hqsge^kW^OX0szI-H^nGKX_zWMpQo%NHn_ziZAQU=cCj}JZBzOB`(p)oq<4bJQs zyX}h!BZFke;sHGyhhDPwin50q-})ogHCk&*l-c&Mj)tn&TyAWcnzA}r%n1v4I=zhv z#y(?|$7KyIZ6+8qmY5{g4ScBn^C^r57%y|d#11+z2PX}A9ZhTTRjVV*xmwCA9Tr?& zou}((kAkuW590!Yne*{M!ZXD^UF@r`KQYk^-W&~-E`u6-O=tofr&^0|D)AYKOshdg zZ6WN|Rb^vm5>Lze258WVoFv%h+*s#7I*+7jteov^T2p{p!y1P;!o8$fJd_C}hYsEh zK-gD&kl9&3OYcJ#`ytcUUDr$~tk1Fs>c6~xbS)dIZCtF*v;I zqsFzvyzL0nZTzCbOdJU{8smd;!HQumg; z^Zu$dB_t#>omQ)ip!ef+=PS|`Om7;LA(RKdtCQ{Y&-I0`Dh(?!OAPp^1TtHrSy0bl zTa{?i-IiaJV-Dm_Bkp4s8x-tY*8=V%dB0H6hInYYfMNp9_E}j>Eo`5vI2>W%S@Rt? zye{UpRLVqtmin{5uh?FcX-D6i9fIu_>=-3jM%kC~EGC}a_^k?-5WT>#t2wv;?jYoX z@f}l?de4@rN*w#W>c@WPuj!9lcnSzNkD6@qdynYLE@l=P`jY0r(iD>mE+{uuCH@`J zZ;{{4SJE%|-~7QB%z_Zxh3ij2B%pAzJ3V`iF*k`zmXf_B!|kji;5>WoW=(jEH!hd`<{o-| zQ5`=2rboXHnsb@{!)@CK%PcPx5V$SU1z=<3wx z@9UE_3bk<)M);dBqtMvDeq3`R4WpnH#r2EO<%k(2l4G6M6au?b;R_r`X+oyt->Pyt z;i9>)(h~&hIcVAMFfq9pToUEzV=SROHEPkZ6iFEA*Znn|E&Q11r`Y?zv-yGQb7rvO zqp{-!+256yb>q#&TUnB(AB&sd1ju=Zc~4pH$bK3;eM9Lc?`oKLVdZ5*FYv)a@TkYO zuzLP?NOq_F%npE$v5B3`tD0xMw4li|3H9s$X{<0{#a&eB7UNo?wemtAH2CLeAXPM= zb5LXdRe3v(FoBFS-wC+LL872pm%dGG5ZXfXb5swidZu&R=~;Wbr&-@#L!H2iHlD1# z5-FOi72_(sp3wNg6PfRaPRFbFE7c+sreQeGu?SZcHVa)672k@l>e<(Ahof!7*?$Bm z)ryxK(MRB!znI_YJ>Y%Y$C1%?qwlU`eDn1q{a}Iaxm>_!DiON=&jKMRj2JKL_^|bB z39BW1@xCN@SXm~%l4kpsR@kU4SLZuDIdW)lz*(7&&Fr94_2RBjWaKz?w(C7uM?>6{ zBct{Fk;UD`+nFP#!y2d=948-@pg!FI(jQH=Hja`Kh=g7Pox(f)-SqUsC#ltW!KIVL zWx2gJP^6@c$7i&CEU=jC@4;1;bp7j9qwK-qGy%d3uw(%5>d)Qyn-j>6Zz6`)1vw&MYZi*j1QxuB2!I zIy0Cdi$&;KpT~K9t>U7b?!7ddNPW;U`|^$bHiescyfr_(O`u*6YnqlYZJs??1YK%g z`m00awSsQYJe7D<1>ZPVIY76bvc~O_fG+%siJy*`gM%6ce zgp*7Umvq#ww62OK7=;1QzRnWPOqn<~h zy~<_`(q0R|>#~fY738gpW+6_~Qw?DUrJ{Kd+9i&{5qYjUELqeK=`uI+N$Xyh>=i!0I%r!AC6O7S|Y~F09?-JD1{w{>Lo=2|khHkqs?BoKHWZ zm+5ez)?4AV9DV&1-f5%2wsoi~oih`34m)hoj@%sy3Z0N!PZ92LR?yT-0=edkl>0OW zHR_mOjTsi7j23fV{NBz&KDIq$H!yRrg~&#a$6wROm@ek!tcYWf#eECuPy5vg&CdfZ z)g;eR`*BnXI=twq?YMne`Y5uPS}Nl84E8kgo=&|I?fw1d^B0i~n(VY!Cf`uN^M zb;gQf#Djh`c;zPPFqzeuU|Z=yjNoXcP%@*>B5Lh zqU6Y`nVX|J!c_n33Uy%T1)yv}|JMboOQ!G|Dtf#mW&5Zw3Qt=}7s9P#HL10&R#^_n z#~ZXaVtFZ84g-(

Y_r1+1|*llFXHa)5h&Job@GK9b0VC7)OBCW?61bnSnCY}Gm#Uc%%-q&x^7GBvk zishvr`(?Bjiwi0&XZ7oCN9A(b`-74HgjGCKzsY#Bnih=Q`Ds2;moifQ#n;n7=|5)& z-%#ontkr8F_G?}Q^G^2|IbZ6OqD6FAshz6qMH(v)Rnz)|tB&5b#tj5BNc(1Da3L|| zNFZwt7IHjYooduDzT>Z$#fw6ttkn#3+n`9*nBzuM+L>VRU12t6eS?u_jtzT{(o=Df zpw&{Q{9Po^g?;0*Y&ukFk`%6j`c>?OAzZRpP0OlqkEcP=W0?3Jb|;}wLEZ+<$#KE- zXAmqbF)F9;!PS7M%Tq_Ps>gQdfBTNLuSHK>rDBN`ADP?#SS+qWT#rhe(B(;zLnM!M z!gJYlY1%X%IgNx0l(b)ob8CZ%$?N1jc(Rw8E1-dgc)~%yTj^S&=in{3wMnn2B?ZOL z6;kC*8`l=!^$Aq{G&{kD+@QFiY}oC#C`)K>m;@V`eHlovhpavW;6I)%yH8T zGDP&$6!am(xdR@lkMPZqx#_xLxnHTMR;0w1_GM2sPG(LT)4c}j>zXholN1Ek-Z+3& zIN0*jW{G;zo-(lGn!bKY17i`+B zZNzIfqL}_>Y2lrk5!;SYi~8I7Phy4<`my1#_x`)f`Gt!7 zO1r-Cq6BYh^BTI0iA#%|`IDeIGF{hf%WMJ%n(BfB2H1x6#;nBj-xof`L=p^+sL+pw zCVkYy18t=#aJ<7+uFXi5DyC!|%2k9Y&NgV{pGof8Up=+qY*ujRp4p)k_yCR1lHyQ7 zi@l2wv)wTmHLJy4ibTV!cmJih-@YjKnWGdEK+M4QaqC{5xuB69k&+FZr~FkCV>hfv zvH!8(o1*GddQ93@{As?K&=3KRhWEKwq^~$z}8uG zU>c2a&3!yH$X8D1FJ~ILA62VOfHZUS()@7IE^_TlDUnd>>2>&Vo+7!u(xP&yPw5o#Y51b;lAZ@{*v|N}n-Rsch3=LbA(ow8PnL?-GP^gyO={ zP7;pA983xypOr{$Ygf`Y6u9XYguSr!_&Oe2$1X?U;d)Oo^m$vV1@tknrcpJNqcVMX zKWoCH5f{`C&kp7Pa6^kRdNXOdaTrIXn8k;2`bLdYbMIUGol_r20z^8!7bZonWEA#y z<53~Afum(1Q^~Nx)wHOmUr!oEX&tIYw?`wSz~4WVqeY;()^mM;QtL5esuWdUJzO6T z)usn*((hpjXBJ92qAi7^l^rHM4_1-~zZy)+`m9;@E%k|e=Cg~um}>2U;O(Tds~P?! z%dx`+5)6F$?vxlG=bec)t;qdDNG78sBd|B_s2xHj z0ERWCtuZm45`-5tbkUdb6xN0Kr6t+O7(~)GV+@2qbGX&9hZlAn(Lo?$TTNz30~Uhx zggF7yi67o2&b;|{Ng>g73DY9^^~!rVax9iEBuh1+LPxKJqY~-CdwzM<3nNlL>BA#H zE$z(K2OptRVoBiSiHdkN#$sX8LmfSP$9*$#ksMBj0UX%-YeBEYcOU$&&?iN7p5i^^ zynsyk2u~JA)L4GYjn*vUdh^+xiGJX>`hJ~rhqK#^Hc(H}_Rs&_#@E0$Mp&q-ZO2Dc zDQn{RPw&FC2G~sEnqTP{&ZRdC~46i3i*~2~*h*!2!CaBdDjCa7CnDcOme2a9x)W<)8vA zazZy}=2KYJo!k9;*9ca)q+fgmqx+uWbuLf2v5ZSocxydN0l%D^H}6#odc$Pm zv|9M1cY1ptY;E(Izdu(d%w$r{a1`^S@jX=Do?z+cGU(dt_x#F21JAj_CTC?WP)<;( z$v>o5Q*>9qakt4nHK1(06&6T>##ew)Pq*J2He$BA>&%Iz^*_#}n`yA;b~kK)k@18| zCzqGMs0oe}hAuy5QuGT<1ga~FVC6+(=teP@4IBu@jX+C#feEeDpr@kvc@+Omq;#11 zW%>a?>9H7HESJ_+98ib7uK>l25XgO!_6{K*n?IJ2m8@FZAEibA{7m@f&y64xe}(pJ z1X`=S>BM0&#|JiB&5g6mV<^G3yd_c|Fw?WJBin~CX*iQ`E#S5v`ry}mu~)kAC{Z$p z^P_gyGnV{7!_uvBOU9LCCN2EJH+R=}jNs5tBGWvONgE~eW)gEa8d4ES!#G*<>q>4zO@`dS^=n>?KxWg5TSWF13+Jbz?Z{lY5D8oE;wKb%6tth42|x`+ z4Bqo-Z!~G`hzuI}06Eo&(BDbaEf?bIhE3Iggh+o&Hn%XxnZ63xBJy9I+UU zx5I9^)nG$uk@UOybD1L;frdI4cfL%gi8&K-Z0q1~@@E-DLq8lzsI?)v$pr+0M+ZPy zqVZdrwb|Up&419hajR#5OB&PV0mqDM_+m$Fb6^SDRE)x;=xD3DQ2@_9MUF%S zl4k(_7N}pr!LKnx)`*zjiYum3#AYO4V1? zMkAk3o5Ao@b$b5S#n`~Vn^Kx-7|hsE^dYyI7#4USBpDI&i;4UNw?9RLg|r5`nXVTa}%VF-?7@Bd8q;*iP zv*e_(At1H>if0A~cC#gMb*JbrgVvhx)Of?;r<Ox{b)WA!F>QuuxN1S`})5$U=rREwWWhkhizjd;cmU;!zYD5lDoc!mZs_ zW7mqdv3%v5=LFe@&IjsCfhwvfU_~U=(0I`!7dG&iOQLGtFvZYwFIyKsz?j|tSx2mL zn3DyviliQLD0a#ZuPp*;(+;Wcm=lWG95GeKa|hz8pV8{*huJMlr?kzP&6bP=7k5NB zR7XEl16v}85eNhNkUd*w7hXU)Hp<=cFsWtn{roHjcut&kiCyqOFk2lP*m46l0=zH# z_m+>l2!YomDS+&E$Uv-eq%Le=S{!F{c()TO@W&Gdvr<^Dg2O!1i!eG0 z(|}vqlX5R1mqYPh@qq8|bOBcx6UkBsjSq=J>`t5n$>Gs?bPPXKpSFgYxaVFA(2b1^ z`Six1fyZ40h#RH_(hT%PC0E0bmc0s;)5%^W>JJ>K^S7{Tl<7K*Bp*v~Sq#0V=qeRv z#cvz}LS0#;$bU)Rj|!Nbi&SPdqEMQ~<0^o16=_!r=tkscCelaX(`5U-L%^M+TE&vb zZ_?rG19mZI!w5s$#{B=gMvXQ+=jg+fH*1o}pP!iuDRoCVgUi68Wcy)KM6^|7C@GRg zfO+A}^L9@HMMQTAI*I0ga^oLTpH3&33}%^ig>767Kc1ba@sIR8ck+ZzzAA(Z$V7x? zO9YtTk71k`^Wl4}kXvkhSR}idYiVSh$P^F#(`l%l=Wd|!u>7T($xui+=`ySN6h&9} zMMIUs)4N;v1 zVi5;MbM;jQ`)C<=GCO2_{4m+vqUr_2{?aroA-FB-o;50LzFJx%KLh`5t9PC(aP)Fe zZ+->~1CKti>Ad7U?&CJ|OHT$`pe74IC(4&!@(R*OKI$$`L z=}t5i9KG6j2Ux*g1i}j1YCtJnzQKFmFSChi$@nT?!l#-ddCQ301LxgMV`hPTSfrsO zOf~}V_1M;*60KVFx3lnYT}Zpn4cXJTEjeZzctZY`*^yA_?{zTQtm|O73$Mm}y+P9E zOw*6mWx=V2ELIUus=#N)H>^vB5GTx?$&^5kv2h}c<~Jn|(W)q)1}^CRJV#HPhKEW< z3-+h-HaTH&heP%(QPMN(`>0igYPw3}cXB?toLRX|b~Nq>+&D*Vzh}~+P>O53Rcnsk zh(kf0Ym+8P@v(zxB!i=Gwi&z#XB@cyk5_Zo8kEaGh@Y(VgYs0$*YNDO%c^ zTHI77w2NT$aM{kec7%srW{c;W9|xS!|NgY({nb2~D6BX}4wD?%lY@jn4B=A!>$OsL zMz&3Ok?GEJ6{s(Vg{ZDs(O1g*8LMjiM)xDx4&Yhp-G?ImBZkBQco0 z1AKQ4l&eE$RLlyQL5-TD|1nLQ-!ZP|Pf->&9PMw#s=zsE+xT7q;fe}8xjIG2Gk=Dt z-(+2slI_r8jpmf4n>^9iX^vO-hs0SOesPIXpc9^-`E!#%h>g7+7qesim3zisaus{K zS#`ECV~rw)IoGPX8x*@SK;b&11uWG>{q;znYD^So6d((BYC7MFXA%hnpzaz^hZ%qmET@O zlgqfoL;j5JnPk>40O}8Kq5OlJD$RQSiW?}~HXD*}`6dr3Osvmi|=@aC_EtC1z z1d^(yhLgsa+TrM>nLrI(%ExSg*WjlHg$lvs=)esQ3XWo5PpL0E_YN*EPSKr?O|(8k zn0=PnH#S9_!udim2i$U;Yc*3ku3m<+ zc|14o=;;s*ucyDr#*EbNaNUP6ZNnllW{9Kgh=YHHX1983S;M)!hJb2-xMRDA*+*C9 zUNw7k zxzP22enbvqh8%!y6X_k!t~Q8sc5<+dHOofjM=gRXo819N?()CG`y9k)d!!vdCFS%c z%2Em-jd6K5RnMJ&=FE7%s5N9Zw85&FO_P{69>1>OT>VN~pL;kBZW;l^bgd|Fbnic8 zXcHz@8#soXRA1~8P!>*sG>n#59FzuF&K-V@C0vsH;ZToLHi*o9lZ+vbGRXfSIyhg8 zhJac3Rlg|8RrSCiC1X6l566Jm^6lJuz{79Z?4JCM?bR2D@8YPwwcv8tESCz3TvAln zI1Lsg=y%wIZ!fcSP+mW#g~(cuK7M29dc=>?!)aP>ESWk}*psloDYX3Y-jvfZ0fXcz zkW>I=v=z6j0>UjhAn&k%#RINCbchPFGFaaZ-#fEtW1ruPuE(G}DwNUDn;n2a+5^xk z$>$~U?Ckh1-iGn*|8zdPCcJXee{V1B+UL~=-uO}AAolK8|8#JT?GX>(>Dwq@)JW=+ z13CDyRiy|fcb>L6J-CEYWH)y?Iz))HcYvuwMm^ruhotns+g2l^`9~aQ1J`7PIC98VOkfokvWEToiN((iwkr#ViY_4E zi{RRhCzjX$aseg@E+_Rd;%`h9_gEK}3q$-``D(w7LpR*>ViJhNcY72WL)!)rwF52} zFWk)awxw?gm#be->*!r?BXF1SNWfp1kpFdoSk!=M$PA0vp~J8qY=(p|vkPFYDR%^C zkN!dlR$je)FH2K_aBWtEKibE`3dFy$w<6wLf4_Z>*cGgO1n7IT;E5+cq}!ozb->$d zkSUUsPixoNG+XYv{-RD>-)c7?} z*IXbaRE|b?w{8@bctBqK3_h%%bsXeWL?EJObs$dRO>I+tj7l5`HZnKL29(TT z7KD&|y}-aM*<2DTCe5e}*@pyY))OT|n*1=zo2BUHifO!nTz*3!%5FeoS1U?XM+_}=~ zk7zQ16dVU!|GCP*|MM!qpz4_TDZQF6%2+q;*n{%fPJ?j|T6DcM*pct6>V@*gw=!@G9rff`6yIS(3b)Cv92 z;^e3CI7pVd@G%LQBL>HpH@=S&r||y?5OO<32O_Tw#_lNz->s?TaFW6F{U2V&lP(%D zls55Tg}%|Y#O90j9}av#UL_YPY5_lZBs@d8>tO(-PvK{B!;}bXfB?^!E6#gMLzcm8 z47PDN&uO#VAmE5*tnjg5PMYub`VMQp+DH;e5kg}!pItr~ho23B-0jxT8&VtVl#s)2 zQQ>ztUMm=_v8G-A5hB`p29T-A$|WzEIDb0Yh%h3DR_s_*^n3v|l1->J@b2dFgkJWJN^AF}EdQ@CCiL}ZD1 zU?YNI4;^E^l$Mjr-R@*|i8@;wjx@cbP2a=hUa7QXgM+T2GlzN!NySI{fslH}`1#sE z9PsR}H}4=8lFX*}!>`InXik&pmkWZGIS0|m74NguvGO5AE%KF3_ZJheVFpzJ?8{{m zbgAZrJje$2C7#4H?W*4$j%gVc?Bnp0=T=|J@Uq{J{Y}dIy6*!2#PACRz5+;^;ie*` z4VYhDZ%KS!Wh@@Y;Xn=>-F=zT0eze?s-vk7cHJ6@D(;Ef=(?=1(OW#_a@pZQof;)p z%lA8LHOKGp?E@1|X*m35j6R4{wM))d9WvQlx}n1Uxk!X|fs$h%QkKHy$Nllua=52J zg$eDoYOl@i2CH<~O zt*E6hhD)4wDXFjtTqSqNgTmge06k)GNi*)i{9*JJc6=wyzo8~;}OdNO|h4z?J*q;K> z-;NTXzm|y#@0ihtlNa`4u9&~7I8I8g<_Q^EHOT6rHoH1#dK_5;0NY%BW?btH31^lB zVY>g)!gYiD(105w_s7n4A3mK@1z51GV#gB`5^>K^X}}2c!!GbRR4qSE6+=lx+c|Fq zD%a_ECl_dQ@iGU%H(S$N)GeV=5+1i>m;t*F50D@&w*C!^^8! z89U>z@tXamj6A}!?Fvxb7q5nKLEaP@W)5dV+o4J)n_)SU>kD|G=?LU)=JF_TNJb5 zIBEm+C^}q{6^m{(GV(rh&cA>%TfOTw)~D3umCsCyDM2Bh z*g;88ZB7?x^uM^`GN_maNp&nmP+PXZq+qVcYB*^vZISDn<31DUc8~qud$A{I^%K}H z`Vgho@2Kya9y0LDnVmM5Vrg_Vh0Ouz11=`p@mdGJ8l`Bs$zz50$ejQ?k$56Yc8!Dm z1ip#3ut1MrWl9j3xxj(}{H{s`iA14chu@jVsffsi)jA_BQhDFT_73A4&n~9-uaT24qa%rV!ppylcd4h;W)#WevkQv=-^us5{xlCE;Fp! z9Pq#-ycKcI38`PkUe-!=x!m!zHe3*-Th{TR*PfV`>F%!q#B%A5vgh&a3XUkKCI~`o z=L`6=EY(*L(JIgYQ$aReP`J>o@WpOH67+>~nx-vYAvxQx!OcyQDik(>6YHfmDnol*O5j%;dEbJw31xSd+=(A>#QgGn7 z9nQsl$FQOtX?Z*OJdUrxk;rr81pr_7G>p8Cdm4qt`yeyc2Vlqm#)?4a2LQN%Z{M^y@dt^yMsZRjFUX4Szh` zSw0-Eb#17r=O`*HHGBeHKP6=aQ))n_j&~++i2-1eXx_ryJLl$WOw>wJr~jYmPw5vu z)1@`x%Q-2JF;DLk%f4e&OJ{Fx1h>&Hc>hca0e;mh3))E!&NIK*9%cN!e>>Ew`K<-= z&AMTzKAw@5ouvi6a>F{D=ZV5_q5{mBcjz``s>#hMiP=ZNMM8AdAc-Z{E2yZWFX+D3tcHij$bxW zs=iUNcB-7|c>6Z0dPoduKVGPgFJ4*4p4gQ&znQ>8>Syhpy@gZuKyd9O&LzOl1xg4j zL4aRoVjnwN6v$@e=v|>~w;p2Fs@zP45?XKtQONA&}0Q?%!f>c{zgE6#_Q@tT( zAbLhrD+`S$Lm&1Fb4PlM2a`&E4;T9@CWF}u%GC$k&8+O^y-W`BY#O)ZsH)h*Zz3G6 zW0RK1@^X^NUW&gS0eA0;4Mjvn%oqi<4(%vFF`_z<#mwOdb2Dg%P z-KHLnar2whL0(mAD&H*LnR;4M?s}2i(29+Gnz%_|?%g4=U_XjVNT9&6y2?;5jHU?X zkN{SbjSa4+*60-R|TPT{ttZ#}0Bouho$ z|AK04KNr9lYz4g8{#}4d3b3t%TXTi^_nG`Dfy^g~&S}R4*UJ&re4xm0*fb|R??*GP zU#<5Y=5Hzyupw9Fc;3qN7fZQnZ8Yz9=d^R2&$$fi$j>VchHV9P4=otRdrV;_P zb{82oQw{uNf=S zRIk-d4;G>4J)dJ(;m6U1C7StFC1?9H3fgWi{nJsP1g_W8vt{Zy^~v_KRgl=;k6T|H zg{@S`LO8P}UN=5S32m{K*xbNc5>te234X11gaBp9RK+I{nRg@Md}(HK?j+rLzN?2| zbSi`@i$4?2{Zp)ovKbw~;=To!y0zGlwDWTBrZO{i-4GkoHn+OV(;o{qL{dJ06j5+@|7F)2zpLQFjJB z#EMGIO97=xPfhKN+|5U5j<|Zd^dsNfK&)N0NKYVXes+AZN_T#<8OAlsv`a5=Ao|Qy z13g-K_vPzvDKz@^+c-Zuy+cF@qt+J+6{LmSoz_TzBrLG8@u|4~Z4NE?M~7d`^>-!M z@)sv2(^8Qt)0V&~;z=$a_!FVz@(pBWdeS?jCbe`pcW&%1R_$5sY0Sjm&gqTDto7<|3A9kI-siV2^W@gC509R zsaG0RdJy)kmla$6A=+?nX*z_trp)7;b!DPiC6&XshP*nB`QPtU>Hn5_1;OeRzT3&h zyuF0Ey+{&_{lF#aY9dF}*~KoUFj-xIi9x8$)CQ2)@Sl8in~WF2oAfiH-!0;XGNtBx zR|{LJXyQ7AhwjNoDzsEYAI|q@y#Vx{CO;CX+VIIF%RNfeK=onp=;YulfH~+&N;T?{ zUYF|N6JC$KL^l|$dZk)##t%h{KWRHK>(8U=IdeK*1D4q|Xsn_NCV&ss$g{{+W`G3fL*{f=7?#CodWv>DsYPngpFG7+nTs!eZN`xld1+DA6hMC+OV+a-X7 z@>C_&{kW>vRurJhW`YfRk?H0Gf9<+;kkvDokwa2=8p1%}93XT0Ks6`|s>Nghh^es$ zBo`1{1@pQ5TL0Jns#$NyyNA3I#gtDZ z8gE7XVI$=zt9VklcmlIqx%C*?RGuhm`4txn5 zcsd_*G{pM>*uBka%FG5dz8);p2SllOjg)Xz#@R{mB!RllsLF%8 zaMuqiHCw!60q+}Non>FlFnzvfO5&yQ>dd#_WyLLabD`~MZ?F^tr!2b1)PrNCxF4UI z!qG5mXtlB}gtNRSkWN_(f}X~#^<;-*hr)D6g(<+>g!>wf~nin^C72snSqwX!+jyitLqPs3-`67y*gKHsB8{=C4O5QT_*>SS6cN z6p;r|jA$YiduQCkd-t2;TQ&clnyipHY&D2DPdJI$fr47qNPf4#N*9Hi+_+5t6>Im% z&p9{_j-MTGHLfd|5aj8W$mNyGOa5>yurKa@7b@!`szCvfj4P(rjKzK_i@Frz8bKkUc-#*P*cmP} z{Kxn&OMOXne-p?PytiEd*(ydAOs>$udo6X>Kp9>Hyr|vC1`o5JCxV_m7Bw%F3HeFt z86+k{F!T;zx$c;D1U)4Nuxb0f)|~Zq2WbRM?<5ChBxP-gGAL(~z>p$HN6o#9<3N+I z{0JAEz@{@#hakGu6Z4eNG0DG*V8)Whm+9A++RFd?8$r4meZp2>yfgC%EwNhgSkWkh z=y;DNvvcZ`Z49kePKNcX28Xq8*5nx>jY}m;oX)2nm>3G#=GjiLlDh?aH59y6!a*)S z*aJ`k5K$s?f<@P-Qu&0ba2d9}oc#UlOhP@;KV)(Sl`G*R)#ihIRkViO_QvKv`6`nB z$Jt)Qb!<`5WG51sac~^>u}>-s4UGm`R`VTNytY>X`)+`a5~iKy#oG*Gr7ko)O;9wX zFNvsAt+x-oOkiQ>ed0{UBAhLoh#pfgDF}{(UePZ8isbaESWznY2B6_65yBUiOBE1u zLeIPW zneW?utu+*e{#KS;i6n6L`Dq?M(S-wv-+WW*aTzY!oMVI#tx%8+>) zp(g=8Cqh7HjPh6jabe^Nr4L`a8@Ldx>)U)xlyh*Tu5Czz>0EgnXM8A%5tsV^q@VyI z2@swVFA@g_Q5DBACB41bbnuyrSUMQf*X$LQoG3^-_D8o(to_Uid@VU_?sj;s+o^{w5F568BuZ3*zA?#8?-aG#anSc7LFeZ3E zk9te1IMF5X67^IbALB9XK$zaYWw)B*@vss#{jYrG)wQl+el0y@)qSxJu}c`FfXDp^ z7hnJVWBx|44Ie&(V%SNR^>3Az!wW}v6R#uNS^bFB2Nh4)sJMK|vBLTrb(Crtxz-USyG zDdh2)=}Sk=e?LFE^=pEZ$Pmcsx8XbWYEjFN1KL-PGZO@gaQ@_gq?5T3#Q)@Uz0i!> z1?2jq9DLHL(ELjaT|K%F5C;EM3jE8r6%Ia1I2!=Dgo}%_wXqQR^aN=Nj6Z;ZE-iRQ ze6RHL=2UtJ>QGW@5Bxev&Hv-CJfC3!0v-8hV>i|y#L5(HNKyhOGu`piiizDImUVXmB>ZHxBmR`5*4+S0@>yX-(%w75A6Hs&#C$uEuDD!FGP= zfp6||2SwTxv&knOuGSFqz7Q{d7v6P;Q>bsw?Q*}pzaw-bnCwm*erh_J&AsDPEkFMo z#O>%-b6#}0aw{{LVwb0+sK;yFeloTi(_1|O`WwBOUK+frloA;yNmmbqo>8ujnKrqM z))p(p?QxU)vF7LFT&4$w7?2yKyMCW7lSt82eZ{5*`BbDG zaQ53c(Pjs4K+RZo*uD4Uig9O(!vjQhD0)I{SRydmflg z7Qg&#OiBAME`T}vnd032^v7iD-4n|34?75@AHQg3j>pL9>HShzLgR1#((YkaDW7IY zqab6OFR5=CQqO&kQuwvO_V*}4@ZloD}dtkG}Fo^I-aulEX7$4*0&$yW` zo?t6(Y!S59=$Ntl(3N4oQDB}BayWwU8XSi?G>oOnY5cBgb)~EGcl>ZB-Kfc+m&7i| z%E?ch_%~@7dzD+Qm+yDSK}lwak^UL4;)hL>OGFaEA79OP_m`TG>2hSfw5VLA)em#a zvckFX>famnrV!yMixRgS?_9BlcZyd081^2oXYld;nx=yzm(>@I3}*3n4OSQ0*o!^V z=5>9HO(RPw*;?pol_eeZI8OEjoSH_;d?Vd_mF;?87*%p3jlQ(Kmj55ntX#^YQ+V%W zWAaQ`U@j!A!W!w}aAe)86;_D0fyJd42AB&yt#_%jNb2rp3gSMl(rY4X+)o`H!_?c8W7aL1w-PTfv%T(V~I{54NA@1o(Q4q%Al0m}~N0xM= zvX4${@5XeUE=2uw&wCFYeIyXkJTuOZklI&w=j|5|==3&lv3ADYpg7DAyB{%E|E@H@ zez&(!1rhCw6+8U$aA{5(w|sNKoD~y?oICfRx$ z9O}mzTxE%>v`*X*09#9kUIjC8X#va!;t zZ(g8074%db>`9pt+`pE838A)7QmG}#a4$uUk}uLkh;c=%H4O~+!x%@&7?8r(@2jB9 zTR-sRxI^4Fkg=`J zH#|Gqfs!*=8zpZ7q!1z(~goYGs=cuKMU3$Qp3RmriSmb8K+y=K-U$If zXyR0?H`|7rv)!jfXIdV-+_%T;BW@A<%u+Q;dE1kZWHhe)zbHkd zP*%$*M*}V$6f$eLAj-LX_S}ArHnu-8urO|LWnpegJa!(RGMO&T;*c~$%V7h+X-@y9 zv84S_N*DDm_{HtS78q;YHhiB9KmrN2kBhQwHh_eGYbA~`{N3$wm~Wip%b1+ZMt3j( zr~exZf#B;B5;H}@5%)vE2jOefgyq`7r+_#ld^6}alK=9HeRO4PRes+pDSii>C-Gzr z9%P}Ja!@^7|FN=kV(^_i;fPzmQ*@CkR5-{%SsWbk^$sRk-qYxpH*Jo+9i~HeX&&pH zYuzF!pL3phJr{nA^qP9M$6_r|1SMq^`=i!(dKNA%ZaWm>_8%PtbV`}{Z_o(yRjoh= zixdV?Sah42=EWdVgFRNVSg@~D^$x9tW9rh|q9l&z*@f?frxXmT)TW@lfS#Oyd`60% z_M03{s5QQ`9k5Q90W%iLrdjP!?nx|tR8$n{!7*h&P;DzCozSQ#$`ehkg)Gc>?XJ|IKz8<45cm z?e0bCtS~qXAQbX|Qh*+UQvv}hV6Yf`ER>cdo`3?1yCI+e*m!@dhD6X9gLmf$FbO$t z;LX6cM{r%cLVx#u`)JTo^&Aa?s<@>@JFNF?)Hk z9n~8!J@)vlJ9|xRWo8iZZ0=U!!S)Hl8>P1twlo=|H%m0s>NQE1YEe$ApDsnRP~{_? zNcv#ao<%-nqvayhWZaP`O$jq(6YeC4|R_PFJpwokJ_>EJ-b z8wYh#l+oGjFvGJn7sLFcJOzRPY|5_VzOZt~Tz=oP!{BcYgu4A%R~Zb~9-PLeRx3us z<=p~Vl{Yxv`I-Pf@d^$9rGYnu z=vzjOs{uT}v&R)Hl?dP{Xz{xn5>FOueg#=>x_di*#(r=1$?{ZbDyPGK{;!uIdMd3{ z{Tj{BDU|}=}2E@Nzb!a4r};XHI-wVSPGK~(~aCEsoaIO z%#a|zXQUg634gk-bSsohz=7Fd7Klbzl2>El6CT>P`!$8Ao$;N*ARV>2`e7EF#7y|{ zEK!6zPN7~rh9+7~l?FYvB`mGA@E4RI18*Kxgd43%*&tLR^Iq#k1i6a#2SAT`NBJ}d zAkas5b1JGDb4V~J0QaYmiq-T-E~Jq7s;kCh6{voEwAOuLS}JvS1q{$&5)XZccpDjI z*@p#28?(ED&UiLs=7v)OubN~Rjd6mo{kDoftAZm|BDhY*raLSBVxe3sOOvQsH(<5N zD-UeN>|_@75RG!GUUq@RPqR&9GuW&(dHm4_EBhMH7HeX9^x3#|<1n#t`Q@{u)lL&Q z*%jvu73%C)IUx(^*e}(^P^&_n+AAb*tW68~d#64L`;mTO;V*ac-=B(|GlA`r0PfRf z@X3W_%wLR;lyGrqgY#OrbSsw-HqVd8 z;?VcX;g;EOAmvG-V=Df|?O|%l^yY=hgP?@kfAMVUxJBuwO0`A61LVuiEu~ZT=@_Fd zMuG##Be*O+Ik9~VuuPm>9%*+4Ec<7GBV1&i5Gu(dKIDIJ9kLrYQlQ7K5L1K+=PM%s z(AbU^xm3>JkNX5|1DP~$LsL6TJt2QcoNi5>MjDZiw0J(jk0D|RocYRFe;$&Z&XM*B zuy{>-DM|^sKmN#wE*^A%Yi7OvN)*1;fyAaqsU32**E+qG~U=igGR4Z^9$^D#d2yXfIJ=1PWV<1kln=L9mq0 z{h9S)LUAEsFOI{ZXzhkL<>j$3n0-cI0W($&26iyr?$!_q9yEO!G$I(zL_i_X*4KF%A9oq=#4MGg)NmgU+16&;YYN32S#L9UjZRYQo!&Nv#PZK4EvwkbNVWKj zAS;o~rd4CRjbp?w$1$9^bR2LI)luW=aT5jk7BdEvjcfoJaKi-{#=1YEu!#oQQ}SL( zHv4|Im4{Nz^TS17!0My$?uKyw@cWZloVmx5W6+0WonT4g`Pz#H|dbaf>3;nsGG0HPLdwag7e={26s z!vVtzhvKvGHqaEe<|F zx1?Bd@BPYVl|am5GC;Z+%rS148`c@vSxFAcv#?^$k-5Qyf%_Vh=|gEFv?lD~7GnfU zS4V$2X@x~kep==$R=$n{RD!KknC=~_r9yM$nfb8>KMFoK6AFb%sd3YM^2-|0?M4B@?Upd7V z7?ET+U7lP;O|L6aWv8rB1;Wi&(TpPZJ7~CMDu_*#!i?TY-4pqd!)W;Q#5d{LW!d46 z^ZKD@a#t86THB$ZA;X+80XnmI><@f3+YKxmoDK8Ix1hx#W=@UAl+^;n8S%u>`uywM zO-?efBEFaygGO{@^~L5qg6+nZ@=fnf1sC|g+dLG>^~E=;j-o(GvD5^~Xx0$c*-VC#0BdPD#+?Lw({n z8@%z)&NwTdVqC*^?*9QIr4s-*Tq%XOZ@aNP5Yd4(ZC_$wU2VD1i31b~$p(4E!{l?ZQs-Wf4Mq=tOUU53MYp0cItiDk?eR^Y)_&Yp+9IkUBr&x|IHuZ z@xzCyHh~%ofSF?=379#7sxnh@5N?n9vF;}p!WD)!)S6WkE~mr+pu!bE<%jXhD|wE? zwm=!jt$IL`$G%iW3Lb}!_})0<_m71?1=U|M;H|%}u6OAN2z&_eQ{5@~$wo(nEf)KD zh^lzCY6&+}A@hSf9|fbV38^v0Yr!XuxCBvRb1e~dlfzR=p6aOjy{vir|+qRaL z;EA8b9hR7~U#+e=P%eMf4AQ5z%(pZVgNt7d)oiGVp5B381yCI2Wp+Lm!1qhBr?qq9 zWZCLLPE98dXLp3uzxb!|pa|M{u)EEk*aooW#Qb&74y-@;susKRF^OaAKWz>H0AW&H zElOOB2W<|fQI~U02o``g zIt1)BQdXGvf4VV1L;C-#8w234`2I0|9|WO1pK_q}=>tAO_5CSfb zBu`zbv;ZTFaFa0j(Lt2I6~TBCpp{gdX87P3r2ooI8AA}Tmq}SY;{p5%8XrhFLE}Sw z#Cqc&k`cbFK$H`80(apDp{)bHNv9!ix~JrKL~&3ytY_CvJ?vkilc^k_Hv}Q>t2~(k z-8!ImTo~Lo&9^?(O~BP@nY1~;AB4yyQqKszngq)D0wLuL%Ie?Wc)%K_fCO&zlO;}c z-pH{n0$S$(3=sHN&p&9xNc5mOCh?Q~W;jW`7LNn7Rcg@`0zgd>O~HWwf#AzgD*-$& zKm|S+baXR5FBydHRCzf8FbRoYnoaz=%KnZXbaUoX*HP%@4y!F@L_wvlFfq@Qz*s1p`bk%U3qSU~N@7V7ONGBvhF8 zn(c82<+QPfZxL(6z$^~T+iN1~MLi)YM=&GqQEiH^`PbgROS$Sf_0KSd|5?iy$7M$H zh{5VDPvRi(PfXq6X48jQzVNtQy}fOeY-`F>?12$hMl}8ZK1KLNRnIF7mqf;o{(ls+ z3A!lk-@liiwwmp=xJaS))VepX0} zDH)QicbtZWr2@t@Sa=}E<3hJJyVkrKw1x_l61%sddTV;1l>gb{&Zzd%j}@*3EHI<^^Ng>9=cBjUc=e~Mk1-; z?PZx%1nSVDZvbMXN1l2QOaL|oXowyX>B`n=`C_q$`oCgFwHYP@rd0;Y?t+>}ZCKk* z@8-9F7^p+RkVzzL7+8)P$s;L6sq639Q`Fg>xtzOV@%r%Em8~GVgaYy!{B{Mh;b1gh zlc@0P%B!1n2_D?O1cdZMet-L-(@Ktw)w`Djg&_EVzW2tzCHqQx0!Z7+0vG^Te6iNi zj`nj>qVltOcKTUIY;aqrE35G${|@@nzA zptL{CfYvPx;vYKmEjUA?9l}57)kHl`qrhn|{1!@C2Fb+a6;~7x4b?H>)I%XfYD*}T$5U~XhXApqfx-&dZbQwZD zNEb;#ds%IfQ!CVLfcNf%OEA4dbZE_DK(r?N3CIx1tMVR$0#!M68joraIruUE zPVY_zItVjZEP6 zyz4&)mGNU3sahV)0-+g}bhG+Zuh|4{jN|jQr`0BJObrr1b;{t_<~?*<37`T43)1>_ z%8ptuF|I}5ZSjp(JN1rTd2?i4xfy&o7+6`^G9=-?^V$$|=H z2qgS5o8k3_b#K5OuPDI~5)47fnVr>eI55{aVsi2^nDhHIS8gs>mQ=CM{qjVHZXC?e z`q2?{4^}J$qum&GQ@R!N5Yt41WecwZ2uWIi1E`Kb1%>jng(CD5LutRIl@%fJQ>}wA z4LRYFN?B3dfrRIGZneus(_$f?L3Ah5fftPg*rM&N2_9U3%Y^jG@sSL)Ce9oR`a~Njp|4fMak5$^M$*S$_fbP6B zBJf77fe?mrFj{Y~$q5eVng^JKiE=D}TZs2bAW?Gg=-WXd7VZD)9Mqu<&*`uF0)-n6 z1xu^Dn}+_Z;>{yZxs*aY44=w5gLh-IU?gJ1+dO&*8`&xt^x=fY{D?--4Y@sI%34+ z2zmP3qm30xz1IQ@zvUTqWDR8#)$bBt47jY1I_&W}B48PFz;Cok%LnHmJu9Dy%9CgX z1Iukj$Tr#%T*DC@Sd;Uxr^re5hIG(hFfO(B-gVpDxLa4{9o9I(ok`;VFnSCH1;Xqc?-AVzu$LM!wxZfsgos6QyvVx76$$ZY9~WIc4C3y}HisqPd0q4s8eBy04Xj@~WrIoa_nHAp zuRrd_(2q$xg6S28UG6-g-VoGD8B8JAXq(Z9KdWSiAzviz^)A*J5gZMp#h8LSBwfGy z(uXy_oYTx>ES@Ym@m55C{zS%{p^B!5#ZTwDTC4_jyUcJ%q5s-Zr- z!re_&xO}iy16Pz;2pq z=!d$CU&txCZ(}hmwjOGn>s~dl z^+b2UwS1pI7!qnw67WFey5`0pE2i76xzxJ6J zgn}zUFPX-_up8=)8?sQUU$+`XLPGd+lp*{4SN7M~r=K$;JHcB? zZp3OLFSvgzHfCByArrcQ@W0RP?LZ~tX1P&dGvQ#rA!VU(n0CGLKRzoDB>M~_E zM)MRXQq^rKrAr8PIJ7z zuz59>d^e&B)V-dKUDc{xe}ef0ucRrf**yaGHIAhLkEE3Lqc^~WjZ06|_HnEpWfP(d zV|<@ozF&L!tnqTAqBao0cIA8Q+3;0&!Z$7d|Cp~dLU7lHVC`U5OgQBU2{s7}30#oe z(;{bv?V|0mf;)_p_Yajm7B*Y%%v5KV7>w4K}l$Pf{GK9ajFs{zp-5cN*PoPgw-y{t-O6bn2W@?$N?ppsB7hqG;Ukc(f zng10XL|=gqTEX8c6^nV}NLXog94tyWlK5Tly)xtOj_B!85h`8td0)s0w;zo(O>kbA z=ZKC{$IKBL{oa&!_L`e!+aKgcnYG};5U@4qfxD-^QfL{%R|yaweYcGFGR2Bd^2;8`)1>$ zFcV|xN*3v^p6Lz-NJS&|T@xEbJwgr_5JHlNP-7r(^W*atQesrVOUP30Xw?R)L+q|( zF?}D(*|eXVDx||d<|r!ZE&qK=>Cj~HS&&1o2GoiPh*gw|J+a5jEDi1i*f|IO{#H3Zsx-HYQu+BV?L?~tC4@Fe&7T|D*&l1pOf>tP zT-g=40{@|M8&Mfo$P0&!AN&hJZI|H&(a@KyhWRi##r*l`)|06fFh&vs-tevcoE+66p{jJ@7!1DFhv zW}9s%FCSbp-fmRnDO|M(UVD-^b6W3F(8kh)dR^I)3pf12P7H7AcbimnW`3=cd2 zAat7yFYW<#`9|`kxem>7DEN@;Oz|xQ<$$Wuirzv7^nU5w{N2U`-Wsn__hS$ zNrPlM2f0?W#UE{BnKmmrw)U6s^W88Nyw@2Bi+I7%o3iIp?{4!1fei{sH(zPmIi=?E z;@G+O*~q-kM;kmG(mtMb!^>eaM%zaps>&z7VD71)xH|cAdV=)zDg65NGhHN`($Rm(abMTu`mI#896o3FL4nc%H!in+ztG1p3L(CL^k2 z8PpUp$IPS^T3QoB9eM4K8{T-*n47AXJX`#ziF1zu$(Q-|%KJqeZFfj7m3hKGI2RoM zp3iAClB|=k)}Xo~Jo}j~{BD}g(}KF+OZ(a9%sp87Vr%iv%$*T9x$EmRV=N3@GCRrq zvESbFMe+1xyV`EY&PR}TH!@B;Kl@_IkI8^Gc0xWw&#S-5QaPH-6CXmul2Lrlqxszi z2Qln=^)@^1b>&o{8UkJ)er=P7GM}Jx8TJce)@TnbIAUSCj(+i0UD&rx!c7!|%Wtcv z_D&-ax$sYSKQfHVR0-!WDewNK>@i--hkKKsXr5cLve&Era zg>^tJFq89|ry04+ohdCwjJtstD%EMe#5%d%*l?zD4#!D82j|ek&Q$0Ml}_$_^bgxC z=>jD=^2*q=EuBvv3qn2i?|-Kn|6WZ&tW?PNuZcudqA^4sizRZM&}po))Sq5N>h(6d z^C-vh10S2(k^kI|62<`O88kKxHj`=`62)*1=Y_SM4eQ75`AKgpBsEU*64`CGyj&fd zQlC^T@EcB%T8*L0TVuNQ=$A8JU#L7A&B7;$V<d{6=&Y8jCvUS*7JuBr^uQYWJ zHdi7R3`m z_UwC_VvZcCS>aqJ<<}piF!dtFl0*dfZg&q&_4R1e=C5y7MZY#E%vDwOQ()W->I5{c z_q{kMA?ThwA<>k9jbPLwXj&+YBF&@0@YOY@LZ$#A_)AaG^)(9JDfnO~KWdCm3kbn< zNstp=f-h)=5iMjC=MxQP;yXo6}_>ky48pk0V|@SFJ5o zR!;s#af55v;srk=e%Inj6u~Be{khr+&T!chr6S+?bJ%2fBcbtKBZk!Zi;!DO)v?(f zPGz+QiVNoW$M?3qQS)e|&a$^RIv==f7-gNqiJNz3Ul^TS>OS}sUfPa)Ini&}667fU z!t%wTDzVZiXFX_8CW7O=BQp(>J)DUwehUM@5;N)b=@FizYIQX*J{8 zEfc9Isgh$v#`fC>e|WFrRvt%kLkSDv^GZxid@~__P!zr)m_F1#(RJV;cCR5xc}y71 z7@RI+QFvJ~^n>XaA4|@$Kmf4H^xplFJ+_&7iQU=mOP?(AKgP#KCCV<vC=>Hz?o zmc;OFvLCbKO`CX8K2KP=)# zBPNm?KYoMBtiPNo{3-z+d71%+oKZcCXWH^Np+?!)9&c35FWXasXdhk{&3hF$+vvP7 zAsFvFp{)1VNluU=)#+G!0-SDE0^@s0b{u&=9ohBM{=86~7=_a)lk_@puuOO9rQTXO zt0b-d6Q8E>xF`tsLH}TP23C*Sbl?+0y5Akc`Ev&etTUsFIi#cW2oxKmMgw z#Jxvo$t0qAf*CdDuhvIqg1yd0r)K=VQABY4ygyIY48@%wBbK(9YdxqvuB1g7ubyA{ zJhrFk>pV97sND?zH7|D8Ip@v50JU>C+W7CdVqxVW>0b-gAp@TBkPo5p&wmMcoTR~t zPzl0vPzgAfz0#$UgQZwjg%6HIOxGtE4ueciZH7~0aESGz+5=lX3_})00z-(C#xwnl zIqD@0wlzI$G@{#FP+Z4$-`I>&CJ(3UlS=nN@YH40;+{9*kC1=8Z54NBw)_SeFU%aU zO5W`E{;2g8c_#ePMw_p7LX{LI^J<1@ueX#vrE(;^c&jGt+urv1Yzym!!y3{HUU^9W zK1^_2JP45kBvPHZ;r?d$tf#n>p77!T+y#NNe8Jbc4-!xu;2W^TCMG@C^&EdMv$%LQ znF23P@)(AELvO5x_XwjY=(@Uq#3Htz!}7@>&onL*&Qt)@f+q*|8j80v{@wU{729gm z*d(4EWV%^A&(y?vy(6j;;;P|;pIxeIaPW!2h6=O-@bY9CiPzJJ0tTBo_z}P%#x-lQ znB+|42K+t4Dp?KhG!y@(x79b#EWn~-dV`CW7zLe&<8mu=jXPSqXP#HMm&p8m(yUu> zHCRq@5$8G_%&+0$@`lI#U*Fl@)T6=0G0b2xMoorz=m=om-omY{Z6Fg99>R?l4@>*Q z@A*I4NFNiJcrm%y>~hvM;D6OOS)Y9*PPC#`R+dOq%@8C6OQD+!ADra8tWexCYsREo z4nhs_)>i;iqQ(fFu~Re0w$_CTkua<;_H^UtIP||s_`qL!;g&G5NnpIem8S@@;Jsfe zk;8yL^a}I92P#1>eSvhJNrU~vWZmoC%#+Mpy`cf}wi;g3_l>l(kR=V{!cN+I*l77j5#klUf z-C>U5Ixq#+_WNY=1{nk;@ueW908;t^hFC>LL-xY>c_{WlM z{rM;q7?T)8S93!kcqNNz6*VBwX_XK)0_@?qKcW&`;xI4=K_|K_K(%ik0}GH+p1BD; zLASz8**t|VI@B)MBRexAa;$-G4;Od`%hgo*7BtSz=fp`7I7=k)Rx@t2rKq+%=&oEE{bj8sQs`2(YpY7$w6VMdSeKTf<)N zhm+H=onN#_j8uYrXphN{^hg2U<}zxi4&IDH7Y3SGw|O0`Vk^*KFpl&WWug58*ZHFL zWJ&y&?81M6`}VLX!kM9q8}0YC8Y`6wD!;-%qE~i=ymT7|>lu|{P%SQi#k@+C!BbWl z0sSsAtm!ICj9a+F4EJK%o^O9?lW1l6^LWI* z*{|i$pKm1vkAG1yqggTx#Gr{zOSh-^*Ml-nWUH!y-&dj3jr42E?t31%d{W*{%Rk8T zEO0@Ol~sRvt;|~08k^bsui^yz>f%Dz&DtI;p_rB66m<*G)G3mm>J7Ai^}Fz#aWWC? z))C}+WF#SRaq0>uW1)g*nh)xbUT>LW7Mo^LUX}SP>5veiHoaC{Nkq8ZGtD z<#m5@w9njNUnRvq+VDJC?pR2C57a{(9;!LQWcJ*~5oy(eW$F0%s{9cZz8dT~0NW*jTO5FBO~SHnv)CJ?3fBy*lz&3$;oRrWL*9HZv?%d_W|jPiJhPX9 zUdKp^LBpdc=Jb^Q!x)o)$&9F)1-3OGtelrO`GMw}!?(`Ext25-*Vgti?|@F!p-4@R zvH#T^%}T5CN$~wP;QN2gu>;*bu|FRN7jbe6PYzZ8b5Ubh`8l}g6_;$-e}}99Io;Ua%LfSQcONx=6X2h$eU%M~ zfK2Q`Dm9Eo39NE7V+25lDZeGeRFVGU*2`cL*r2)wz`I{_#z}t-GKcXFy+iw{iq+XV z+SuUYbWy4d(lgR!5FD@(;Q%P#`W=v~n?@+Gc5)_i#S4fAHk&(W{kQh;w0KN=|FU$^ zezI_Q<0I|Y>IhTz?QBc|l_&P*Tn*y(6r@(GdSttGR6}!U6XmY5y~Ut4 zm~+)3ophp0{n2l5Z)NRe-t3mvd&DK5mN)iVs!rJAP#4=PtbfM4DFw?Y1@)%bz<7wy z2Bc06M&+)!UVWn#4``}a_%0IB!4&-#S=u{Axw>#QvDS6Q%~?$VyG#uF0B_!Nsn`Jc zBv1_yT-w~|2Nt_pv2l~wBRwj|VCv`CNQN?#0&WZHZkVRFT2-WY^wNetomvI%?K({y zJ*DImW+f9+XtKsBrDM!1-l~U05z90G z^_(Q_l<7CefV#e~_u7a9Z@oWe_Zf?O!R%J68e*db1i#1gj~1biYpK1%wnm2yAjQ~X z)b9K?Fc(25_KB@n1t(uIXPAg;L(nnnQ`~a=^Dis46NIY_Qy!ZkM=~u3pHx24gzq_r zhYzA}bTN9C%Tr)Jf$Ezl(_G2vVvA>j zcin5rE$GQ2NQ=)Eb6l!v$w34XF=RPgg9zh!krC18vPLyOKaag%Md_V5LRr~}b8^z> z_LF8WOYnOL z^q=^kiK` zJ?vdmcblShupF%MA5+=?iwZd`lGd4pjb00Wsp2V1MYS^3_R1u^@nqG7#m;j#O+DRXmE; zhhQF_AFZw0qj4G_oE#8dZ!k!Ly)WPKt4lV@+QO!T^=Tk6Uy8|ZdlT|eHBR(o{y3(quh9U1QjQvce|UTiCbxd8a-s}Ef6%9W@# zqRVDd%9)jyf?`X*b!~jEfb(dz02T_Ejuw3Oz|HpMuIg}(i9WP%EV@8Ho<~yJh+U88 zljx98CVk%yv+3g?U0%sl?OTr^t0y>BW=K-GgTRnn{$%ArMicHD&krY48|~RLwB~c~ zH0F5{LUn&ptV#Bm|MWxV;~P04p>op; zERYGKUu2**eP_)U132eMjx7x(1xV`KraMbhQOJ(n_a+{*aFbNx+12cKr&Pv~i;_E! zQaELbklL3hpsS5$w|T!DVz%$nuY7z3iYIFqZ42YNriq6Lw+%K=!4Pu%V#e&>m%4_1 z4F%1vgIaA7bG#j-BaOSTly3B; z{zgvNAd^%Kzc>mKs%X_8t9`r}!tVEIjc!9CQhDmfWbr(oJr)*!hnK1UHUqkg^Cv_w zw(@rDI*p%ifitu&yAEf{v7CNxqJ2{RMe`!IC*>6p6u8ECP{(69Sf*kh$@=h$2h=1+ zz7#96!rCCH(Vm)hckP&5ReU*_EjCs7@)zM+l)jHR5pedW zC_DPT2u^5cyT3HCVxtxPT%j*CG$~2TI3Id3el8Bl5wa2c4uTi=DcG~8e z3{*DLe-?Z?hIMLr&#Q^i6uq$TCDzNg*X=X_>4Nx|pb6UWZl>{=P44tu^o8e(qT zfqUX&q+I!i($1CKz#M~8e6%JpG+CiiAcq8&^RAe&6i*>G`}=E)dMaTR|K$Q!jKXfz zj%Ww&vV)#{(*gRMkb8nfIN||~5CfH4`U9{eK@rE$h#b2NcxBHMU5BlJLqWA13Z3Uh zsXwx+=f*d6?S5QBib>%f0A8rKb4|1p0S*sZ+wWpOI{L}l#AO!xo-kgk_z83SC~977vokE5sk& zeK>pyp+_}*ol~jyLE#3*du;K8VG!P;()W~2|B9Ivow7oY7*#Jf26Y}+zXo09=z$`K zKke3Te?V~F#}RCd6e}ZC|8zP!jng?0t8#*Kkiu8I!EV-WcqM4R5mfhohs@iUE zR6hL>opIo~AS_%{(_AtnD1mHO1~2K_DM}c&JQJ&=RW2peSdtty-Ly#2P{T z4tToq%Z;N^C%veb;)c0s{%Py$)yy=WN>`FiVP-X&WIt?HzdFQ1HOj$ymwyop($@z!fk%@k`p^cCR z_gN-O&F|$pY$rRVip8;V;kB_?#qoQ{uh;kFpJ1E2FlojG3rs((uRLaR{jg#^CV#m{ z+_YXr*pq9jSx^rx88uW#nUC9H3JdnuC(SC2C%-eJa>b%i4f_*;zzBaNgL>O)?;N!^px-;8B> zUwZLItg%2Et7=!ceW*cZwD&z@Cmy@@CLI^b%^e>F9)o#~7G=C33NMppwBt37i3Vj+ z>8}`6D+a6Li4}>V^IT2 zpMx*Dmo8S+FCLfK|KiM6986K~GzX{2%=C-XH&Ph>^x(lO6o@H(_}BL&51$ zZ#*gS(086ZFPXIrI_x(BU%rc*GnFH*iihF&0dp87!VTJgQJZh@!lUq3Tj;fRp1rg= z6LQJXAQ0YCdzDRzm3Z*xQ{zU$m;_<*GQbw}VBF;7teS?o@n)T->(1z-x30U(r#}L5`31~>NR!kyl z43kDDS$Xl#u8nBcsou6t#c8L*(;zBDxCl`*g^VBN* z(y$OGUI^1hhZL*l(M!XQRpuH2aV3hi`;*gx0zxl~_)QZg?tFh?L%+Sj_3fZiP^KNCoNYx|Hx9zVZ z*QH{_Mz`cWG~ZPEHh1|C113{{A+~` zPIjenSW8h^WH(GbQ3{8Y@lK&)T}?hCMO1kN{rTZt*QRrumJAY!N}oUMc;B9H%0M{` zAPNHAcv-B^4D7x4hAKl}d(rg91_PYSH`S9jIeJe%*G9KLvY+=0Ny%o<{dDz)yQ~KA zdN#&iFl(y+d-xpS&4n$=ijVzK-W)g3->p1sMt3!}&=W`LH*~i_s9~V0d`4uNPPO&j zZ2xvN3hTfZA^vyujX{doBj?GemAlzw*3qu#z-g9~Twp?)DK7+Ey6TDEoZmk?b2*J% zcHX=B{o3KVlI{l^zaYlO_SosFz{oH5v;p69s42ak(SAx0-+cpIty<-z#fsTfU5Zn0 zuH1S#yH<5KgkI&%(<*JtRAi~FMhgC(tEC8YrwCQZl1X{7u2pDTz~?7V8%aOu$VyG1 zow8o(-e)~c|}}F=_<#EhKHFE(QCtvGBq%e z$}TTvGn`O6iQ#6wM{0}u?6>9P)WdYCkR~|M^M$KX;0UlVRut;#u!csKoXtFjL2Opc z%nltmL)cO}WUx-1ExJOQ7dnUpY-%p&69N`~KkNFnLADuHJ&Nk{V=cm=+zV$YQvqQz zXSUIrxbqNl@$Gi{m%f(Ilgpo`r7b#27E$7jWM6BI{!qDoh}e+N-lm8kj%MTO$$v1+ zc7@8=#gRh&h@ynvEG$=;1kZH834n0M8`Ha4`%CA=&2de0e>Gb@7>S}n2lPu1kHIfq zer-j*fUy&6X5P&qlvZx$F-=W@;ycXBEUfKJlmpS?$qxXdljlbm>RIMS=TcyxkupPj zA(;Y=&_~9ghlQBR$LgwDxrGjqK8Hc*`f7yzfRsjnr;g zMoG{p8VfUx_lu&4(wzY-ysm4{g)n{JSN3g^mfnfWLsn9*=HDt=Lx-wYlO1=}DZ;1t zR^GJ0TycJ$S6~n9NqgS}A2wHo!QclU;qZfr`l>E+WPm3s;5~T}G5NN6L1uH0b3gXv z63?_m5U8LtjTB@yiKZwfVr|xzdX))L579Xnuzd;Ml{2bR7RtQfF^Y3MaHGs}m@8~7 z?;t2OlMQ=mN0PHDf@iA2TE!VGgso{fZ$`-`&WqDS*%4JS0RVnt*(9rK#B}<^)uJAU zYWk&d0-sQUmr%v3XDGuV&fa@R?`pAcGhKY6H3F-zqxf>sejnczJ(;B@ird6siQcMf z^-WvSsKyE5B)a#xmK|oi=L^BKUPJPlM8P7`EV>j(vFUPufDvZO56KiyI`LVK*}T*w zDZI)kneLS?6>1ANy4P=Li_n6viy66@4appM^1c$0HF>Je7E?J9-OE^M;l5J&W+~3? zpwhFpUa=znzTWxd4H2gadQnsDfl%f1uEAy?Y-g}B$6!Cy@&9l3uMi{DLg6z#sQc-C z`z3;XveZ=k9gA`V!mY;q(S-Z%2kk13o^&m96w_bNhhC&`3cO1bQtgQVzR<#<$l}d4 zWC4;&&&c{GD=Cjbxx;sLb)>t(o_Q$8rG(;aE@kwx#H4X-@9=6$^ZlE)$0O)8Ta?ps z{m*WX#`k}RmRTE7=;o*w+ymwjjKuFa8UUFzVmIm^?F8vya(`W3_=@D`8&ioSHO5Ey zi=mji+tOxSNy(DbDD{hYo@(*r5xBAr9ZD+^Nz z-x1=x>ce;+evw43g`IhWIc^HWbRkRJr9GJVpH2bOh{c=cOvPVh>1LR**7xFYM7(9j z1)kgYxmIGLo?3>9MZrNx;$6*nEma&vaM1~L8{Kn;s*?#sk)L{sH) zI=@+}7?a~Vv?7o*qTaUQIjTd?y?VtIKowWg@LTsXPrXl1t}It9&E`0EN@l;@hq*Xk z!E{%NZ!UJ)AC9gTiZ<7lQY_dOJJj!TG9-y*n;)YQF=A&a4hML;mR}SO>-?ql0qw2i zV4b+nyb=i-QHP(xv+hU08ZKxaJ8lj(N0~x%#aoF9>{8(LT2%z04A;HcC7H>%=n+WqI9@^4*z^N{`4FpSuZ0YnP_}aq7|}B2G*4-|4thiRk0=ipJ_guY z6vmsU86yLM$p>yyy*#F6R?UqNC9EA|`3&HGqq#x=*PG~bR2Hj!_!Ljp1+j<)JFYcb zNb7|Hm&mF(J}wOqsoQ1V0$qQB6y`GI_-|aMg}GzB0llrc@Z{ z%)W<_>vc)k+XK1GPl-W4oYBFoti{QF_Ru*@mx28F4)8J|EUPWldLI>lGc+2e?>1Mx z1EvAE^U+@w!m=a>EkPrs0ZpkujB%bQK^Js^YY+59GZt0AlcxyBAI;Tj=~MI7gzITk zt?=B87?dME<^vFt|9Y#Pc7YHFS@cfq+ymO$A>H$OmYSM+`?T{JFp^?`E@i;F0dvzAof>jl4}YV zkt|kBWn8tsNB7=%>Na-#+{Pq|1bxp1UHwuE)fNEF%fp&Kby2g~eAKlp70U_be00+F z!M=it(ktuO!+niYuMs~pG#-FtpI9S@t%%butak;<#*dMXGBujTiIEA^+~c!{NGk1N zJW>5tCKYI63L3@30h{j>3HDdW^JewcmKi0OO*N}&^ggH>{u4lk2EBWqKs|1+1+o-L zf+vR=zG~-(pL@@k_LVk_=CeNBp7pnar!w7S@|Er6Qe#iy>bGzwuM2;~_-YffrygY9 z6Cg*XaGytpQenx+GYIkPq!bZwt;4O$Zg9V)EG<;4!Rnb(s>o+gp8Ry<$t7Mp)Iu}) zI8%|Fmso17F{r|R7xG6>X;v10oiLxDH~3fk^kBOOAP$t9p(=uRy%9w29CMib)Sze> z$1)itKL@L!x@tLk%N3UumeY^rqGi6+sS0xgjwVx}fh%*yN%cmO8Sit*rMp9@X;;)@KKk%Lff6yd^%%VYwAzmyZ6`@h zs%z+RiSu-p<)P!rp;WoetNUp_T81-NFPIMQzOnsd)*{yo9OI1Gt)1mAxu%2fEX~^{Q5#nNp}Qj$q0F>R{M_P6ojh4dn~fHGwU{-l+9ll4(>=g&CIi zXS=!wjCW%|}JQV2WDa+O@7fK5b{OXzb zCFeq#Mj=5+_spZ1lj|y~Izi01u7k>)3^(N2?`~t;G=s4>f1JVQcLQh_06{1@IyE~0 z!ij7%2cnJ_#8w^0Yl4Fn=`OkhyFue8SGJN->$nNWVTOZBrUzpTqo($b`?=SJH;X~p zgPOyY&)(*vL(ucXj76e@MJ<;d0ogMT^vK|ecJWhwWRzf_DqQjZ{xp+xIaL$ofPDJEz4#>qc}(iT7YyXG+yQbM~q9IKB+H?Z3HA5x#v)gFpy(XjjW>TJV5lW2@M#y}Fn+&r@EYDWc@v zr|bUVsB&XsMI5iz;}$bZnfNm(u15W2fFjjka$ow!oQPw^CsQ$Agp<8t8DZ0isq9Rs zo&I6ggN1NIpSRKRMXgw`I}=ZlTibS}O(gwGPDneBf3OeqR{N-?+Xdq03FO@x_w=x`9L1r0f2wd_w^6ap)efM+a7RHZ981H1a1@o@p9os zVP7pAowEZb#I_@j5pE`!1^(v=Mb#2Y@EENT*Z(0RT!3fa#fHLP^Z}@k2~DpOZuqkg z5QXeQNwe$*ixcR<_a4B$XOvOa4Q{f*g<;1UrQaCfBNIUYyTz-Q$OP~4UoN}UOPU@a zDlasH(L9{-U=f7fOMtTyjin9Qv~KMCjw(Q|+?Lo+vr z-wJ&JyCpmFu(KCz#4o|%ThOT}p8tw8k`*HaGq2CVi3^hlOKjlFpzp3i5Du0BwzJ5x z3$GH-Jw49z=^PSlWW(WG``HvY7;N61Z z1Igg{z<)jrz&b{z)UbX4EH(cTAT?VLyIbK4%mc2Cuy{lyVBeU6TYGj4T8UuKE*Qb4 zb1|6X)`_lNL_8`oSb@7l><- z!6)G@7#yP3#}}~1X(JpF0S1oC6a#*t1sV^PO1*&Zi7?o*Teg)=fm=%AV4FSa>wEzp z=`z?^{7Sk%GQpj2ss9qfFqydzeEddc-(W5NHAN*3@Yn3n_RHx~D@~JAf2zUUG?cjD z4C3L6lmYymqc!M<=HbFfVQ)8l4#Myju!h;D_6vByK>NS|0Ni#LMz$(N*ND^)8ye3MT9i|nFsy`My&kjJ`^ks{1FJn~Vk-dj^lcfij>944 z;DDi1;i!{Q8_=y}QQ^Rj5C@<>?@Rua{dN4@B#mY==hdx1q{he-7SOS``nVqF=jfh( z2T1*T4G6fXktpa{qJ&*3NL->`*c-3BB$@eOPIpc6L^uQ#K#Bo;F};%;tjCvFF*PTZ zR-D&K?jG7Ed{yyGxXEIFgOYYdP11+VP#ec}(obr|=UDwlvVasUcVxqsvKayvYx{?~ z7Cl}`c=JeFV++MFz45Wd@?fJpd;4{rYi{kX>eHX%VUl`WI6A-SR@}QKp06Kh|Fzy> z;AIx+U#PN_FVS7>wMhy4Q{H83Y81?RlI`y9My~!)gVdJi1sqxUL~>ibvYqllu^RsV zSs`CBqZ`y3#fzeo4;N;*#;fQ*jjP!Rs1;^812AUYJtHW*Yh9rAFZKS~!r9c08?D+d zwgyOys?Y^sHaMRzFw4>UZvJjn9Piw^wb*i|zVOA?;ZNuFfr8pguGPg8Hc2SQQN>az z9hdl1IB(;}0Y@H*hahb85IIZXnuN>m20b9?^RM!>j<2p)yXWWmq3BV;x_renX5DI_ z&=FH(3cvn$9~E^one6{CCg&MT7fkbsdxCUhVc z?=M%SEi})OqhUdVUac}M27GRk>Yuq7*<<)j@e8P10r~Ol-&|@wSPoV)i;(gV*$JLD z%NX9KFR>UNGZ5VJAdMI4J?GscBjP_s;vJpIRZn_8)#1@15R1_;Cu{YCNq5uw0Ig;yfrA9vEdJl8;J zJLinDldJ(!#sp7PZ^a~$M;P^$erJyF&k&E+n5Z-(=iL*|AKIL*Abu_(fpaUQ*Y~`$ z^g9rUCE^s!dbdNt=YNHo1eYxQp5Jd_SRVMHYe+Uzg&*KqDIUA&I#6Oo`sLYHzOP@| zo)Mjo{^OCpBLsO{ss0nZUoR z`iS`@iXYqLdqR+V)vV#oi+f2Gn;|UPWhOwruO@x;_Eg`~a$(T2yWnZQn-cS_m!_STlji2m ztNw8JX7ZhkKQWv%3<7d2jc?&gYeeq9Eb}m3IX=sBAhRNgDx&3NM*FVqJ)_?@r3QM9 zp0=My*$1ocss{&G0UXd@)Q=bqFtG9-OU;YZlWY*?+D&I?Z|RvrZ!s~%rJ^|E1J28 zVd*RYA-)fI@F^LHD(to7(0fiLG#%GQo??2GJ;oqp4Xx?9dkOUub>k-JyCEm9J#EQR zZc=S^-ro16BZVFkw4d0-4-_iS%GOwFH6#c!T8(`>0hS2TDu=n|w&1f!219WkE@QcL zp#-_Gp15A*pO!E>mM-4RZKTWs3P|>42+?XVa~R^4e0*zFBD9-LoHJh?OY)@W?IbP! zSiew+5Gb(v@Fbr=!*Dw+o+O{ekS-*@CL(ndVnMQM!7 z+tvgpf?McScBS=XRA(DdZ1**aEBBk6Rap-NK=!^dTVWKCO@aFeL=YfIh^Qd#<$N_w z{2&`ei;j5hV6r+?0}8bVC%H!WJS?1YZDTOJ%lwvhARNTQOqfrn1cK?_vp1!E0F~R* z)o(DY1tL3Nj@ix;4OP)x+v}+xmRNL%o|iGA75@_pVBA|`d066kf#`j^I557l7@X@I zA>tF%x%B-|w$PkKQnK={PF(Ke3Og6};WSxdZl~tJx3Omf2Zla5w3(oomNmPC8kB=ock}F27L$bc#VR`&Vj`#4HNB7%G+5#1Wh)>=C-Rdl{{OilLD)?Or@JF%UM8MiAS(1HhEh=tKPl({eh*c zj2gRi@A-=K>5J_M1)AiF`(iH#A~{_)))4MBuI+y zwnYejB0J&{JR>rCN%|7f+b#v=z<-nh6KH`F;RD#9m<-~Zyn&?HFpjk_ih;`Eo*NWA zH~ZNfl-W_6(T?9uePDz$H9q)#n=u+2N+~suKnb1?^q@ALM8_J>C?4(Ph&M z{-4zI{Uh`Q5i=p*n^mv!-Tf?`DB2ChWNuKkXQVZA;{ivGg$#QupgRgey0PHa^kYAnS_F3~0)bY&0*qC$={skp@%D$ zV|n;N^=Ch(9Iu5~Fjf6-Q9!vn2^E!lL*IMSl?+I3TmE!zZ+s6oPT4<^;V*H21`+km zG)GFR+Xz+w;JZq7 zZ2Rc=6n<;hN!cxgldL?VLl1aX?y>V@S8_L1PO;+6#X>Wc4eXoa{48-|M3CPV~f@MzDLm+(y z_V==dBqy~Yu7w2xl*E5=T1r@=5-((!oSC33#{01<&(L7@XuxaXnYG62%NTTz%VTtc z!Z`$oG`%$jbE)2^susCDb7~Od2$Mu?e3DlX(E=rMZh|-4pcw5%r2;3DlI3#Vq@XaK?vG zSn;e*lTop=pFYD5g%~V`=@bnv_6i}sy#M?KMH;^fq}sOYz>{Adc*KmVz<1oHxmk!{ zBWRbW+{DYOHYZa+cU%LYH}4b}(fkkfA4C9wt!1p6_ozbbFaM;9InZ<$yg@tt<5>l` zKmy*v@i<<|&9f!>tahN01hYP*RK4SYC8lBoq8ltg3f7^L2Y(q7TqFr#{<{?QDdrg+#$CV_|s8_Bvn zEI?2>OB$vk7XdrRLLTdHPoU0LgV3k|c^*DydDtCZpn;YU>(GOl5JNl#K~YAcb}dn` z1$bNdPYBpcK9wq%$7k~YZqCVOo&09_R1u7f5EzafTcj;wfr_UW4!3)}NlASSl<*QP z@Nv6pPIdVB8Q|j=K9PwF1mpLIjlcRp%LR6i1Ma9aGS_^~4Z941Sxv)PwDR;>uu(iKSo#PV+>5W2#GGC-( zI`9>RCP^L9SmB*%wWJ)>1w@u z%E~rZpleZMH(~F7IFV5@IoPGam^yrU1Hu6CX3%P}C-*pZ`!o_t-aP1^J05@k0y-es z4s#9B{wtMH`+G2LAgGt_u*BHK&-;fXc{qsmPiMVJXP55P3(rSzADsuu$zb6nCcQIF zJpVBb&FqwDzPJAS2Z!&K<9chqlk!kig@6F0o@nc2ly5iI*zuN=-t{;<4vQFL z-+T2}jwXD3YGAmqL)1A``60$uA%5w(APjpxD?@iVOdiS+OuGyxS|Wf3f_OnsJXM&% z915N{LR{Hh3-Z<$9>vQ7*~;h=NLQ&^&=O$+6MBLo?3i%Wm2ptRslQCK9@G)3#M{O1 zqv4PHAZOkS4z+HHcetGJBhCSgg1~O!X7O4Qepnh2*RK+Ja{-ii++Wi&-fQU%8QXa1LDga@H9cBk>=7$pnr-n}g zfGF_uAUMafq9dnPu8ipW#{zNI@h^t~zPp14%v&Ri2ohF(seD9mi@3OWFi+{TG^N&Qn!RX_ zecEYPi~;!~$u)O-!5biIwzkW{R~;VeCmtW(E(s%NVB3QLqWizxtY85-3sREElC*Ql z6=W6;mnI%%f=`m!QRmVc5D~B`tlq)1PW?}W#-q+japXx7w(9s^^I#8OiIL=KKST9Ygk=L6OYLg_uey!0*Q zvj3%l0?mK(*;_9rFjn@7KGWHxJHU~s-R8hYcn!(!Qen*)~Zn*ZhXr~uOR zUFef|b_ULD#X`>54DHW)wc!naikfFmiegZ=roxnALe^WJ|4KV}+ky&C< z8>|(e17WnoaKpIWUk;!OFLV?5fguJ@1m^`FE;z|P2{&xWeHBlqkYW=fGo{8@vOFSH z@pbA8IH`9S05l=#WZI0>>3j(5{!Xxgog;_={HR|qe$xO14!|LVU2dJS{%=hb#0G;LdPZ|~ z!UtObE$iIL_X;9iVfFTO+3$bhkCZ)e{qu(%Bd*Ut%;XD8!$QnUh14*B(8Vw==3~nL zd+xXY=@jN@mg3CW9iXHJH;E0=?oK!!bNg!7=_A03s76+{?rVdgLs%1B6rQ)3?_{Jh0yq>pEe4F(E&iu#T~vWe1;vK z?ay%H?$;1S2jeudFm7tgOOOP+#{!xFsgp-u6D7hfm%ydbJX}8e!|voY#~}qN_g@Ie@(yMtlfAF?+{}rgz z2&9Ycd|@pTxO8zJA+@p{Yq)Yc7za$(IMQe>}x&}tw55Bbkl8_|9Mk%HTXPu4Y|Kv3RY($B0J~aVL z>bu%f^z6w488B1);JNT&4Hib&N{?o9+eXk<9ppVNOI z_?}XIb7a3e5+79QlPEN2jTF{o(?4`Bmy|Ad_|!aIwSO>T_%cT=ar^`O^+Vem)Zj{o zpFdR_Jv{axt!bh%wI7Ki^TnvtyffJHcrD*&%0&z6j25!(g;(T1y-EJ@#NaEn^bPmq zl8d?bGlJ>y0_m#Pd$1CS#ln8O=P$P>%W*BjAuTt}VnSY*$W}wyGL>WkFH2uN9ph!{W{4<-OxL0EvFVHL-=Kzsc=(TO3{lCY#M>^*$=)de9N8Ns_smBgriM>ji|y^?A}3?A_&PD=szPP~yMR z_<*Al@%AZff`gIIxSM+7pRSgDc3d8kiq(jsm5X3Ap|7r~TW*=9>xTtR}?4>0-CIMf9+k@i6V$O|cxkb6!H; zo-27cJm&BNN{mO14az}v)EoLfuaH*cSdadl5qsj~$l-gn`G9P)V%e<8#aSZhm^fmH z=JC6|Sxr1Dag?2%E9~dXO8QF$TP=2X#%t$JXpMe%t6Yq)avCW}@ZN#)ia5BqUvFn= z>~t`Rj?lYD3&?GB@ElePL`yb*Cwf2#P7f2gtzsMjo(1hQ1VqyY3qB6_`J9pjkI%# zB{{jt6EV~#@trx~+Yv+f>)Ce~oB6fx*(8E8yR|8^32qIri@i7w!p=-Ol#$ zqPDxlB;s@bnsmc?KauX-*vRl)zcCZ#59{1^;O3_!%7APOQ@@5crgmBb$nEZFmV3 zlk-w{&K4O$2YJ*|V}d0met(3^u7AN&MpJgAJorU6NAk2LowSwaypT z^;R3yWD}R{_J&PaC<>*LnT{Kr)>d+J)KVGWFNTBnmiNPVoQj}#tILB-g*i4$W-9!p zs=B5Jr@yen;!LJDn-2E!R;yi4Fj9ksgWY#$EGNDW+XTGo(rVOV_GM6_WZ_2E1{5dX z%9(p}O~2|hRnV)&^RIGP-V-RJ+PPbK$-(Jm zCbjB$GMJ_8$8#RfvD5Rr#B2F-yqoV$tMU`|?@YFXzGIzj7x*1dPYZAJc12lqvf_Lb z-)%E^G^sv&9~{hlfrmbfG5?t;DEjIwH}QVtGgXehN2WcA_=dkzv)?l{I*L9)J43vV zHSLJO#FgE6_IRt_|EuArFg|kJ{`n4{`)TzTqY$FuIx1{CgGwU5TYiM^`5J9r%M8}) zee_MqDBHYpAZIEgwz-%w3^a)+3xGHvio%r&H^`|CC11R_pfDbx5!0ie{MiT2(Y)ou z$>D0~Du%Rj65hpqjj@fMp>R-XbaB>c8Pph?-p$SzaSQbzG|$AVMp&-l^;z;o$FJ5^^v_io4kY~Jwv-0N_Cq# zrg`6*pO|k?$T@^}LKm}0U#wb>e&4Hp)Zr`G?5VJq`erlUFmvUx+0gTm{+;QSKf+$G z=1a2`PPb5o&-~5T9Hs)d3n$Am&1dVrtJRdgS!ou(@VIy-;JXrHNSL53szo;~GHqI9 zND;F&mnL{m8x?Vj^4xOdx12}9!HTSQ)e^bYp^6N?uXojIaewqUy)tKu@l0#gyUTCi zUvEzu2=~6GZ)gy*yXRUZ?Vc=$pyTSt-SZUJO=`Bup%|UJc7re=pUKeVEL!2t_j06o zsPMH9eqINw9(7X&c##XLC9R`->#yZRj}6L04)jcgmp zN45Q=>D;3NM0dFyY*7_c?dOiS%o`f)hmW2f@rryY^h+k7|MG~<;Q5s39Q)2zkgLL$ zxb9VqsEFIQmEM$RJ1&>5V)MT?@A%d#_=e|x38G_rHq4TY^1YU{s3sQliJNB;=vnKF z9nV(eb%>B@+<&D*D($vm+1P98u_0vYk(3q$IT<#}G&JyTlSm^_Ty1V_7^1GJo4+@q zLRP{2@=l{q(LL*uRksUC-zzgk*TOOBWaP;aM@nM@(o{M3YuP>&rKHWX(X~5p^ncx7 zC;a{=zR|`UYe=yJ57TzM`{QF-(KRZPlLtIP>nCn@D!)Hc46+ND6*29}2sjyua_!Q` zI3Zk8&ehD3)2q3AEivZ0d6EBURN82f=}WY@`uQZ^)3@|Zpn{)qHSzMdtVOp$(kl=4 z$=cTql1ph_D!USVyYe26ChC^>h-kF$ni~Ux&F1|jUgc@wGOCaGYCLBc_ihoe>BePf z3O{sU!i8Kzf_^n!xCQaW2?nlbwje3cc~kg4qaEe;U;1IukywnsXp1x@V)RSN(wz(o zwJ}*_;LLWW`3rMda0L~11(ylx@uGI!#`+qq(0YM@KIs@&)1Ht=!pzcazIeS3j`6;;dO7&|ICDrUbU36Dxxi>i5oc=Os+85;Irj$zJX zi?yLS_L3k?q%W2A9Gx~vC3GO1cV|yyptQDX<56)&)U)KK-+C3l*gH{t2AbT;TXRsz zv4j@wYx3%xQ?n#eKR5CA{z0mVa)exYPD^uAo%x~ zu$NpzQ*FDJ-*o*hP2#|v(}nKfH%_O6(zi*)ib|2-q@8W?LF_#S5m;%+X)aBttf^JU9Lc4S9lfFBW3-J;CQ6%8s|4*9p|!_Ak-BOSRvh!) zOSz!m9!~?Ln#NdrCmtIH8x6PyG47CL&8C+>8_^^c<(X}F@!D8cMtT@CR%@$w1EOK7 zcIV{tMr+!xclr{C&AQ~-yVb|_m?g6M(Xs@zcTGNJ3>G;nH*$+^FGDZxy+>u&{9H-% zR?Ll%QmFd+q1MGO8&1C^kDA&|SI+edD+FDyTgBrGseW!D%aDNz)S{*eji7mLY(M{p z9$T|my+rqI${^--pU6^9jm&O#8u$o0>qD1&NgaLgA(mw*VFua<6rA2`wq^3wi&O); zX7lX$Tys|gE_`xFb(MJ=V~;Jjrb-$U&*sm16g!?p_u2{zr+VwyE7Z;o>KnWM1QBe( zz1+qAm2wg&ofZ^Dab7wVk#SwKeQa^==7ygNn$A4U3lcpwtNuB0S_hlnP}esfey9x} zCqMr7{Pu8PbD;DW-#QU#ih*X;2D9ds;(eQxVG=Lg97toC*v4`GgDeHkWmD>dl48}? zhxYT*hig6Ts+t9FT$Jg$KciuXzLG0KLO?FfxVPA{s$NwqxIXRR^8jM$b)5ETrPCkD z7rW_3hlkJkcI%ru)^vG?JUQ>Zdn5yLlHzI)Hkn)rzF#md)0OQT_027NaoBiv7ffX% zL0w6dQVv~?XEdqtAm%>%FN3tdA_k&n`;JZf=yMl41s|y@pxnP0SGq*en}5%@bsRD? zUF6f8rn4-yYwOumzBSsxE`)q|ibeP}C|GukePqAn{HJ#5`Oidi%DV7rxh8FK`D}+# z)N>==o983B2INf<*=27}EN7apy(f+dSNz_6AsEFdYg`m>Qc{2j%CdDus%8~mQ`))Z z8%+^dQjo{Y*FixA0#~Kat0(igka`30sn7Qi?H_B^n>Vm%*NN}S=TRwIXPDlc(wIn@ z+2HaG@sZ9ZQ%hqqPk^dtlLN1xeOAZl&Vdu-8^wxE?c02@x=hRQWG24x7<-yDZq+Ou z6X8-FetX&A%F-**dAZ<@tDc>+?+>>tYw3Pc3-K4i$ZXMcl!(@u)udYg%~{i7X9+{* zFa6Ukp>a-%n=oz%ipLbn}-7}y+rkJ3X0f`TGUP>4wLJr}>M zctI{pYFYVx)PXfh;J0uzmC*X#>%z5sF3b&Tou4mayN@AvC++Bo93{n26^!rGLtXeC z-#dIjPQ(*I;XI{hn{^vvy9jo=Nx23-6cH95NvO8w^db9&_5IH?9lTw8#j6(=J7(vT zM}1Cqj?bkW*qL-1A=H$v-d5aTCt}o8UuVgG<49C1>Yc z`;q$XM7HU$qA2Lbws_ZEtbwg_qSR&+s0O@STcId2DbU`$?22L@qp7MZ1D3mtHfpCI zBWrqSlyAknkG=5sw?MdFNm#@Vnemy2IpcwFTeOG_wxGUNmfqR}E`_kQq zh-7f%b(zk=_lqO><%i^b-Bp(yHggvbVfy+-*{e9M% z{q#R1$3`|~Z&Q2`);VF~SL4nYU(Xczi)=H+z3k!o%>`Zk7*LUP!2h?PZ~@zk?Bt{^ zK`e(pU-58q5mS`0&05T{sRy2{*4ZY?F0pp=#04%`Azc!vNk!V~dE*fxCJe|s2wKbq-qx`|AM&i+6tDQ!*waxt1_DC9Y z18<+DrgUhRk9Lq)!zp{SF+QS$wJ)wQ@>;AlxJLXzNAKSQgjfx(=okdtL9I zd6n1si0@o&Yq?v~VAb(zaiKM2h3^m|(g5w9pTG zK5lXDy+d-bb8I#E14VFP%01SI!31*r{_^y^W=^=z6Js;mP)6y-bEsgT@WI{&rtsS0 zm_?*;{#TMBdTdft2sf)J7PhC43^s%tJEhj@-T>-zAO6$X#yhuQsI$*uJ35z9y+O{{r#YPUK6)&LRNvug_)0pIhIM~vF9*y_F&=ovE0ZO)9h^Ii+& z0$Ly}QYRy4TQr=Pph&I?dggH%k*wFD`;-*O=cn&W`)9yI0x9 zZZst{UA@U)CuAePOHU4$DtXs(&?ci`G@|&~6&Rd`G9@|`#sdfTOV$Gut(pe!Ut?Ss zMy#uKc^YnJm3g_6akR5&5L%LsS5?_+H?i4=wQDbaov9e_I2JXptF-6+vo*hb-Hzm< z`vHWE5hJ3R(jc@VEmG~cjsGk@-pT5>y6cz}&6Ewrqvv0LlqM*qMj?q5h`M#p&~EBU zvPKj8rP%jn+T%|JzF1H6>Z>=nd`({29jyrsn4fXvH(l)X5<~YsjrZ1KO!>%An4gFj z0ufM=6yPjV76(Y!4AOX|ZqK&Z`;aO`#I}%%McP@DMK$C4Qthtpm|p`YkNK&*$Jmv^T)4G%6KTc|JZ1f`eEr$jpm4bZ8hgjo8Gmj zRJSjR+-OQF*ByR#^ykBvSQFw#Jk3ewAL{M@+YPE2N?Mr4k|l7Y#xJJ5{vM*)HQZkK#^ovP{-+K0LB26li`EsA(XnU${tb7$+tR46g?)QNd@$!63O}ah_@?gvVtvoDI(_#EgGvQQ z`D?$&$hPD;bzJD7&8>ca!w*wY3=>7_I*6#6eiTs#zXsf;S(k`l^`)2@ zIA(Tu*`i;(*v?TFzwaIY9S?MJ<|;3#R+v}}IEi*~c~-ie%{_ZhW z&85AEDi237JVpFMP?)a%N2v6$w4ywX8c}0;Ai;gKlO!pW!JR0YkTnV0UjPrUhlHdHc{`S^`bf$=of)N)tpUW;iI)TLJUnhz`@8Y5$ zfw8fC=bciV`{`+#X81snm+#9ASrHGjpIi?wUd~Vy&v){e-}rX9Kkbg@ipt57GlXI3 zHy_fm+@n*B%Dd+B%LrBN6u{V8H!F z`eXmT%2T=Pf*(KKr08IBs4X+)L?=YR2qd9gEaD9;d8`s!>a0#_au0)MzedU~P zmh1(()g6siu6(u}nTdVV*yP$U(s`ag=h&mnlZz+zov9e8XPRSEmEss?8D1y*V#c7U z%l&!m5Ym?;xBip)M->bqj?ELrh*H}l!Q|q>U;uuNt^lfBhETagPmB;?9Da``&{0wrjQt%IC>B%EEdJ_6*(RUt#fEd*#=gV;Ty1X6ue zY9b+=i|4F(PXPP=gbu|7o*N^R6SMiMslBRJuJUfyxqF6mikG|RW*!V3`j4Tqm<}f- zGy|L+@FveaAh>^wF+m)`H){mqmzypf#jC!?N^A9&jRv}2cwM$`6(C| zDeTEaFW_1&y#yVO}awaX6pyNxJ_{P`X{LODXr?oHUSK{9DlY42!a z@uQ`TPsz>V?iR6OP2$gE4$aI;ZU;um-T%0Bpfj}I=KE;Z=eso9&K6lxeO_bwX2jkc zu3_oqchlN$r=5@d@~2az+sg<8?(LMo5}DGI%ZU~Q-^>M#xCb)fdPn&*wAQ7mH(d6A zWt)6R;ZbaJU_qo9KD)W)8OM|uFy(Iy&vKWcf56b>wR)%hN>6ej5yMtyK0wE9LV zCH3}48{H4|g=i}8+3G(A)cUFiWp(jdZdDpKH$u;juN|McD=2ti$FOu#W%BO`eyni) zUJ^a2)vs+}tuHU@9Vy%2@9g5Mo1p?Jro{b%{_U-_f5f>xan{h#FwYk)^RngbX0UP1 zjxAM)cgFk&g|HG%6!?sNnEzYD&9(JQ-T^-SDtXTMq9ZzQDy}$RU&~k}y7VZtTo8O! z)&9<3|4}S0M4>ueJ0s^{PN{X{^{}$UmsQ`}GK2Hv@x(f;e#rXiGmzxXz|HvN}68Kg+Yel)6wOm=izKk8bvPziUUx_rj0& z-mtUm*YMa}sXA}6#+Y=#`{UTPxII|BD*mu>jR6E2R{YW(83-H^Og!vOgU)459H46rO_3#t zDyI}KpDf>LT(OMd=2L;9JkJC9B5pn3L~UJT^7@hd1f$UoiNU0mhOj*{+NxD10-A-& z;^dULHy@o7bXT_r?US!HIPI(IICiVA9-ffVx1oCD+k&I|nNz(N{qZjL8`>}E9Tdjl z#YzhtHla+Uc3c9#d3KVZT-~FpR`bU##d42ONU=Ta7A7WqXgUeDQ*fRmk*}!B~+`ZN!B&5MkG*yUu~)1FZgvSnO5AXP)@Exf)eYO~;lf0pP$GhMk%nCuF-DeXZY~d80pzf#CXRn*>TfOOP#Xa{fU`QPfV+p zru8l)_B&J+oU2x7T3&0~ySZ_iP^E{%r@y`IjAG%;CQzhg*Fzzs$o+6fI(?{o`yZpk zx8z5*BJMA5-A>J1nc5lC{CLA*x8Vy!^?)|{J}a>wn_c9Y)PX?LLqrF!;Pd*2znbRN z^6w0M@wI&E$7|T{T%|y^TB`(qW#QSfUGQCzhdXC`B_?R%wUIX13qO_nMTi4wGzp&@#Up*nj%XS5avHE0HTBIRwU{LP6TuNE3PS~mNqalv8guackt6>U&ar1>hkMbUi z*rJI8;_jdGKG~DqVDjkfKN<324uY@ROcUcXqZ&@OX;0OU6g=RxpU~>c;5>upk z_qJBr;`{^k>5QLB&s44ym<5q)4R+^Gpr(Ls3{6RfU55o~N=kqR5R=Bq4ei%Hisjkl zKYm!`e>+L#s9sQ;lU(IrJIZ~q!_ShLQV81-h$_-C)oq-p+6dsD;eQItbWop(7j%0a zMr-MLWZk!VnSGpGzX55T3pUK$C2<9{rs+b4yGY}`m0gv`ypz1(P_9`+ze76}Jw{LWJ zE$E;rvkOGnGdvvB8*)N~ER6t>+c%|>5iS>k9-Yapo{Qf20QG_yz^{cuB5n(;t-mX4 z&B^&nuvP#S2pkjApR`zvB-~QtC@k1(ls`KVi8lsv;FUJDA)&RK`%{3^%Qi1sn-Li^17KLd)C{-cuJTOigerq zYcKw0T*LzwZL~lKmGdx#fP*ApAh$_d0PRJEy;?*JXOQ?Ic&zA5Yn{b;y4IgvR`%t@>-geN6m**5 z;%2~d24xvlMRo!>mfW31l^+aTBF^h0DtdOmhsxZ*#A{gw(rnDC1@%P{b z3VEj|+|d$n74XKny`e}`0uGsV9VwK~WP*vtw;werG z0?uOV#d{)>#K9I3_K0eFggc&Co;pDjexc5JO^v(H7;c{k9$-UI)(A#UlWJ$04Ug7pUk1p-!GIhFlhJat+W3K1Bs7O z+Rn6lCC1U>@0WjfZ2xRM3BmL@ck@F&v>|6#E^ce|QKTb&8{>ZvUc{PUOK=fUm$o3x Q1wX$SnHv@zJbwQF0MTS21ONa4 literal 0 HcmV?d00001 diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing new file mode 100644 index 000000000000..bdb2fb8e9d3a --- /dev/null +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -0,0 +1,49 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 1 +param set SYS_AUTOSTART 3033 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.4 +param set MPC_XY_VEL_P 0.2 +param set MPC_XY_VEL_D 0.005 +param set COM_RC_IN_MODE 2 +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +measairspeedsim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start fixedwing +navigator start +ekf_att_pos_estimator start +fw_att_control start +fw_pos_control_l1 start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/IO_pass.main.mix +mavlink start -u 14556 -r 60000 +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 50 -s ATTITUDE -u 14556 +mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris new file mode 100644 index 000000000000..ca5415d11045 --- /dev/null +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -0,0 +1,59 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.3 +param set MC_ROLLRATE_P 0.3 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.05 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_FF 0.1 +param set SENS_BOARD_ROT 8 +param set COM_RC_IN_MODE 2 +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +position_estimator_inav start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter new file mode 100644 index 000000000000..2bf7a7836814 --- /dev/null +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -0,0 +1,64 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 20 +param set MC_PITCHRATE_P 0.3 +param set MC_ROLLRATE_P 0.3 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set VT_TYPE 0 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.05 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_FF 0.1 +param set SENS_BOARD_ROT 8 +param set COM_RC_IN_MODE 2 +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +measairspeedsim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +position_estimator_inav start +vtol_att_control start +mc_pos_control start +mc_att_control start +fw_pos_control_l1 start +fw_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris new file mode 100644 index 000000000000..a893f0dc5241 --- /dev/null +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -0,0 +1,58 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.15 +param set MC_ROLLRATE_P 0.15 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.4 +param set MPC_XY_VEL_P 0.2 +param set MPC_XY_VEL_D 0.005 +param set SENS_BOARD_ROT 0 +param set COM_RC_IN_MODE 2 +rgbledsim start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +position_estimator_inav start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo b/posix-configs/SITL/init/rcS_lpe_gazebo new file mode 100644 index 000000000000..6093991c1364 --- /dev/null +++ b/posix-configs/SITL/init/rcS_lpe_gazebo @@ -0,0 +1,66 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.3 +param set MC_ROLLRATE_P 0.3 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 1 +param set CAL_ACC0_YOFF 2 +param set CAL_ACC0_ZOFF 3 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.05 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_FF 0.1 +param set SENS_BOARD_ROT 8 + +# changes for LPE +param set COM_RC_IN_MODE 1 +param set LPE_BETA_MAX 10000 + +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a + +# start LPE at end, when we know it is ok to init sensors +sleep 5 +local_position_estimator start diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim b/posix-configs/SITL/init/rcS_lpe_jmavsim new file mode 100644 index 000000000000..9b416a0b26c4 --- /dev/null +++ b/posix-configs/SITL/init/rcS_lpe_jmavsim @@ -0,0 +1,65 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.15 +param set MC_ROLLRATE_P 0.15 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.4 +param set MPC_XY_VEL_P 0.2 +param set MPC_XY_VEL_D 0.005 +param set SENS_BOARD_ROT 0 + +# changes for LPE +param set COM_RC_IN_MODE 1 +param set LPE_BETA_MAX 10000 + +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a + +# start LPE at end, when we know it is ok to init sensors +sleep 5 +local_position_estimator start diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros new file mode 100644 index 000000000000..096f659d65ae --- /dev/null +++ b/posix-configs/SITL/init/rc_iris_ros @@ -0,0 +1,70 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.15 +param set MC_ROLLRATE_P 0.15 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.4 +param set MPC_XY_VEL_P 0.2 +param set MPC_XY_VEL_D 0.005 +param set MPP_XY_P 1.0 +param set MPP_XY_FF 0.0 +param set MPP_XY_VEL_P 0.01 +param set MPP_XY_VEL_I 0.0 +param set MPP_XY_VEL_D 0.01 +param set MPP_XY_VEL_MAX 2.0 +param set MPP_Z_VEL_P 0.3 +param set MPP_Z_P 2 +param set SENS_BOARD_ROT 8 +param set MP_ROLL_P 3 +param set MP_ROLLRATE_P 0.3 +param set MP_ROLLRATE_I 0.001 +param set MP_ROLLRATE_D 0.001 +param set MP_PITCH_P 4 +param set MP_PITCHRATE_P 0.3 +param set COM_RC_IN_MODE 2 +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +position_estimator_inav start +mc_pos_control_m start +mc_att_control_m start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink boot_complete diff --git a/src/drivers/airspeed/CMakeLists.txt b/src/drivers/airspeed/CMakeLists.txt new file mode 100644 index 000000000000..81a8f6111378 --- /dev/null +++ b/src/drivers/airspeed/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__airspeed + COMPILE_FLAGS + -Os + SRCS + airspeed.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/ardrone_interface/CMakeLists.txt b/src/drivers/ardrone_interface/CMakeLists.txt new file mode 100644 index 000000000000..70f50865ae42 --- /dev/null +++ b/src/drivers/ardrone_interface/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__ardrone_interface + MAIN ardrone_interface + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + ardrone_interface.c + ardrone_motor_control.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/batt_smbus/CMakeLists.txt b/src/drivers/batt_smbus/CMakeLists.txt new file mode 100644 index 000000000000..d1ee69d45d9e --- /dev/null +++ b/src/drivers/batt_smbus/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__batt_smbus + MAIN batt_smbus + COMPILE_FLAGS + -Os + SRCS + batt_smbus.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/blinkm/CMakeLists.txt b/src/drivers/blinkm/CMakeLists.txt new file mode 100644 index 000000000000..599913acaf26 --- /dev/null +++ b/src/drivers/blinkm/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__blinkm + MAIN blinkm + COMPILE_FLAGS + -Os + SRCS + blinkm.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/bma180/CMakeLists.txt b/src/drivers/bma180/CMakeLists.txt new file mode 100644 index 000000000000..7df83173fd38 --- /dev/null +++ b/src/drivers/bma180/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__bma180 + MAIN bma180 + COMPILE_FLAGS + -Os + SRCS + bma180.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/aerocore/CMakeLists.txt b/src/drivers/boards/aerocore/CMakeLists.txt new file mode 100644 index 000000000000..f1fb63a2556c --- /dev/null +++ b/src/drivers/boards/aerocore/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__aerocore + COMPILE_FLAGS + -Os + SRCS + aerocore_init.c + aerocore_pwm_servo.c + aerocore_spi.c + aerocore_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4io-v2/CMakeLists.txt b/src/drivers/boards/px4io-v2/CMakeLists.txt new file mode 100644 index 000000000000..8969b90a29e2 --- /dev/null +++ b/src/drivers/boards/px4io-v2/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__px4io-v2 + COMPILE_FLAGS + -Os + SRCS + px4iov2_init.c + px4iov2_pwm_servo.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/sim/board_config.h b/src/drivers/boards/sim/board_config.h new file mode 100644 index 000000000000..6413bb780ba0 --- /dev/null +++ b/src/drivers/boards/sim/board_config.h @@ -0,0 +1,13 @@ +#define UDID_START 0x1FFF7A10 + +/* + * I2C busses + */ +#define PX4_I2C_BUS_ESC 1 +#define PX4_SIM_BUS_TEST 2 +#define PX4_I2C_BUS_EXPANSION 3 +#define PX4_I2C_BUS_LED 3 + +#define PX4_I2C_OBDEV_LED 0x55 + +#define STM32_SYSMEM_UID "SIMULATIONID" diff --git a/src/drivers/boards/sitl/sitl_led.c b/src/drivers/boards/sitl/sitl_led.c new file mode 100644 index 000000000000..53d033234f97 --- /dev/null +++ b/src/drivers/boards/sitl/sitl_led.c @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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 sitl_led.c + * + * sitl LED backend. + */ + +#include +#include +#include + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +static bool _led_state[2] = { false , false }; + +__EXPORT void led_init() +{ + PX4_DEBUG("LED_INIT"); +} + +__EXPORT void led_on(int led) +{ + if (led == 1 || led == 0) { + PX4_DEBUG("LED%d_ON", led); + _led_state[led] = true; + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1 || led == 0) { + PX4_DEBUG("LED%d_OFF", led); + _led_state[led] = false; + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 1 || led == 0) { + _led_state[led] = !_led_state[led]; + PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF"); + + } +} diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp new file mode 100644 index 000000000000..d07ec2468482 --- /dev/null +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -0,0 +1,419 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file camera_trigger.cpp + * + * External camera-IMU synchronisation and triggering via FMU auxillary pins. + * + * @author Mohammed Kabir + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define TRIGGER_PIN_DEFAULT 1 + +extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); + +class CameraTrigger +{ +public: + /** + * Constructor + */ + CameraTrigger(); + + /** + * Destructor, also kills task. + */ + ~CameraTrigger(); + + /** + * Set the trigger on / off + */ + void control(bool on); + + /** + * Start the task. + */ + void start(); + + /** + * Stop the task. + */ + void stop(); + + /** + * Display info. + */ + void info(); + + int _pins[6]; + +private: + + struct hrt_call _engagecall; + struct hrt_call _disengagecall; + static struct work_s _work; + + int _gpio_fd; + + int _polarity; + int _mode; + float _activation_time; + float _interval; + uint32_t _trigger_seq; + bool _trigger_enabled; + + int _vcommand_sub; + + orb_advert_t _trigger_pub; + + param_t _p_polarity; + param_t _p_mode; + param_t _p_activation_time; + param_t _p_interval; + param_t _p_pin; + + static constexpr uint32_t _gpios[6] = { + GPIO_GPIO0_OUTPUT, + GPIO_GPIO1_OUTPUT, + GPIO_GPIO2_OUTPUT, + GPIO_GPIO3_OUTPUT, + GPIO_GPIO4_OUTPUT, + GPIO_GPIO5_OUTPUT + }; + + /** + * Vehicle command handler + */ + static void cycle_trampoline(void *arg); + /** + * Fires trigger + */ + static void engage(void *arg); + /** + * Resets trigger + */ + static void disengage(void *arg); + +}; + +struct work_s CameraTrigger::_work; +constexpr uint32_t CameraTrigger::_gpios[6]; + +namespace camera_trigger +{ + +CameraTrigger *g_camera_trigger; +} + +CameraTrigger::CameraTrigger() : + _pins{}, + _engagecall {}, + _disengagecall {}, + _gpio_fd(-1), + _polarity(0), + _mode(0), + _activation_time(0.5f /* ms */), + _interval(100.0f /* ms */), + _trigger_seq(0), + _trigger_enabled(false), + _vcommand_sub(-1), + _trigger_pub(nullptr) +{ + memset(&_work, 0, sizeof(_work)); + + // Parameters + _p_polarity = param_find("TRIG_POLARITY"); + _p_interval = param_find("TRIG_INTERVAL"); + _p_activation_time = param_find("TRIG_ACT_TIME"); + _p_mode = param_find("TRIG_MODE"); + _p_pin = param_find("TRIG_PINS"); + + param_get(_p_polarity, &_polarity); + param_get(_p_activation_time, &_activation_time); + param_get(_p_interval, &_interval); + param_get(_p_mode, &_mode); + int pin_list; + param_get(_p_pin, &pin_list); + + // Set all pins as invalid + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { + _pins[i] = -1; + } + + // Convert number to individual channels + unsigned i = 0; + int single_pin; + + while ((single_pin = pin_list % 10)) { + + _pins[i] = single_pin - 1; + + if (_pins[i] < 0 || _pins[i] >= static_cast(sizeof(_gpios) / sizeof(_gpios[0]))) { + _pins[i] = -1; + } + + pin_list /= 10; + i++; + } + + struct camera_trigger_s trigger = {}; + + _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); +} + +CameraTrigger::~CameraTrigger() +{ + camera_trigger::g_camera_trigger = nullptr; +} + +void +CameraTrigger::control(bool on) +{ + // always execute, even if already on + // to reset timings if necessary + + if (on) { + // schedule trigger on and off calls + hrt_call_every(&_engagecall, 500, (_interval * 1000), + (hrt_callout)&CameraTrigger::engage, this); + + // schedule trigger on and off calls + hrt_call_every(&_disengagecall, 500 + (_activation_time * 1000), (_interval * 1000), + (hrt_callout)&CameraTrigger::disengage, this); + + } else { + // cancel all calls + hrt_cancel(&_engagecall); + hrt_cancel(&_disengagecall); + // ensure that the pin is off + hrt_call_after(&_disengagecall, 500, + (hrt_callout)&CameraTrigger::disengage, this); + } + + _trigger_enabled = on; +} + +void +CameraTrigger::start() +{ + + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { + stm32_configgpio(_gpios[_pins[i]]); + stm32_gpiowrite(_gpios[_pins[i]], !_polarity); + } + + // enable immediate if configured that way + if (_mode > 1) { + control(true); + } + + // start to monitor at high rate for trigger enable command + work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1)); +} + +void +CameraTrigger::stop() +{ + work_cancel(LPWORK, &_work); + hrt_cancel(&_engagecall); + hrt_cancel(&_disengagecall); + + if (camera_trigger::g_camera_trigger != nullptr) { + delete(camera_trigger::g_camera_trigger); + } +} + +void +CameraTrigger::cycle_trampoline(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + if (trig->_vcommand_sub < 0) { + trig->_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); + } + + bool updated; + orb_check(trig->_vcommand_sub, &updated); + + // while the trigger is inactive it has to be ready + // to become active instantaneously + int poll_interval_usec = 5000; + + if (updated) { + + struct vehicle_command_s cmd; + + orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd); + + if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) { + // Set trigger rate from command + if (cmd.param2 > 0) { + trig->_interval = cmd.param2; + param_set(trig->_p_interval, &(trig->_interval)); + } + + if (cmd.param1 < 1.0f) { + trig->control(false); + + } else if (cmd.param1 >= 1.0f) { + trig->control(true); + // while the trigger is active there is no + // need to poll at a very high rate + poll_interval_usec = 100000; + } + } + } + + work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, + camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec)); +} + +void +CameraTrigger::engage(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + struct camera_trigger_s trigger; + + /* set timestamp the instant before the trigger goes off */ + trigger.timestamp = hrt_absolute_time(); + + for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) { + if (trig->_pins[i] >= 0) { + // ACTIVE_LOW == 0 + stm32_gpiowrite(trig->_gpios[trig->_pins[i]], trig->_polarity); + } + } + + trigger.seq = trig->_trigger_seq++; + + orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger); +} + +void +CameraTrigger::disengage(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) { + if (trig->_pins[i] >= 0) { + // ACTIVE_LOW == 1 + stm32_gpiowrite(trig->_gpios[trig->_pins[i]], !(trig->_polarity)); + } + } +} + +void +CameraTrigger::info() +{ + warnx("state : %s", _trigger_enabled ? "enabled" : "disabled"); + warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2], + _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); + warnx("interval : %.2f", (double)_interval); +} + +static void usage() +{ + errx(1, "usage: camera_trigger {start|stop|info} [-p ]\n"); +} + +int camera_trigger_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (camera_trigger::g_camera_trigger != nullptr) { + errx(0, "already running"); + } + + camera_trigger::g_camera_trigger = new CameraTrigger; + + if (camera_trigger::g_camera_trigger == nullptr) { + errx(1, "alloc failed"); + } + + camera_trigger::g_camera_trigger->start(); + + return 0; + } + + if (camera_trigger::g_camera_trigger == nullptr) { + errx(1, "not running"); + + } else if (!strcmp(argv[1], "stop")) { + camera_trigger::g_camera_trigger->stop(); + + } else if (!strcmp(argv[1], "info")) { + camera_trigger::g_camera_trigger->info(); + + } else if (!strcmp(argv[1], "enable")) { + camera_trigger::g_camera_trigger->control(true); + + } else if (!strcmp(argv[1], "disable")) { + camera_trigger::g_camera_trigger->control(false); + + } else { + usage(); + } + + return 0; +} + diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c new file mode 100644 index 000000000000..507d1972a71b --- /dev/null +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file camera_trigger_params.c + * Camera trigger parameters + * + * @author Mohammed Kabir + */ + +#include +#include + +/** + * Camera trigger interval + * + * This parameter sets the time between two consecutive trigger events + * + * @unit milliseconds + * @min 4.0 + * @max 10000.0 + * @group Camera trigger + */ +PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 40.0f); + +/** + * Camera trigger polarity + * + * This parameter sets the polarity of the trigger (0 = ACTIVE_LOW, 1 = ACTIVE_HIGH ) + * + * @min 0 + * @max 1 + * @group Camera trigger + */ +PARAM_DEFINE_INT32(TRIG_POLARITY, 0); + +/** + * Camera trigger activation time + * + * This parameter sets the time the trigger needs to pulled high or low. + * + * @unit milliseconds + * @group Camera trigger + */ +PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 0.5f); + +/** + * Camera trigger mode + * + * 0 disables the trigger, 1 sets it to enabled on command, 2 always on + * + * @min 0 + * @max 2 + * @group Camera trigger + */ +PARAM_DEFINE_INT32(TRIG_MODE, 0); + +/** + * Camera trigger pin + * + * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6) + * + * @min 1 + * @max 123456 + * @group Camera trigger + */ +PARAM_DEFINE_INT32(TRIG_PINS, 12); diff --git a/src/drivers/l3gd20/CMakeLists.txt b/src/drivers/l3gd20/CMakeLists.txt new file mode 100644 index 000000000000..453429569481 --- /dev/null +++ b/src/drivers/l3gd20/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__l3gd20 + MAIN l3gd20 + STACK 1200 + COMPILE_FLAGS + -Weffc++ + -Os + SRCS + l3gd20.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/oreoled/CMakeLists.txt b/src/drivers/oreoled/CMakeLists.txt new file mode 100644 index 000000000000..f0407c71f641 --- /dev/null +++ b/src/drivers/oreoled/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__oreoled + MAIN oreoled + COMPILE_FLAGS + -Os + SRCS + oreoled.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/pwm_input/CMakeLists.txt b/src/drivers/pwm_input/CMakeLists.txt new file mode 100644 index 000000000000..81c79fd842d7 --- /dev/null +++ b/src/drivers/pwm_input/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__pwm_input + MAIN pwm_input + COMPILE_FLAGS + -Wno-pmf-conversions + + SRCS + pwm_input.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp new file mode 100644 index 000000000000..373e7505b6ef --- /dev/null +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -0,0 +1,1037 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 hil.cpp + * + * Driver/configurator for the virtual PWMSim port. + * + * This virtual driver emulates PWM / servo outputs for setups where + * the connected hardware does not provide enough or no PWM outputs. + * + * Its only function is to take actuator_control uORB messages, + * mix them with any loaded mixer and output the result to the + * actuator_output uORB topic. PWMSim can also be performed with normal + * PWM outputs, a special flag prevents the outputs to be operated + * during PWMSim mode. If PWMSim is not performed with a standalone FMU, + * but in a real system, it is NOT recommended to use this virtual + * driver. Use instead the normal FMU or IO driver. + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#ifdef __PX4_NUTTX +class PWMSim : public device::CDev +#else +class PWMSim : public device::VDev +#endif +{ +public: + enum Mode { + MODE_2PWM, + MODE_4PWM, + MODE_6PWM, + MODE_8PWM, + MODE_12PWM, + MODE_16PWM, + MODE_NONE + }; + PWMSim(); + virtual ~PWMSim(); + + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + + virtual int init(); + + int set_mode(Mode mode); + int set_pwm_rate(unsigned rate); + int _task; + +private: + static const unsigned _max_actuators = 8; + + Mode _mode; + int _update_rate; + int _current_update_rate; + int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num; + int _armed_sub; + orb_advert_t _outputs_pub; + unsigned _num_outputs; + bool _primary_pwm_device; + + uint32_t _groups_required; + uint32_t _groups_subscribed; + + volatile bool _task_should_exit; + static bool _armed; + + MixerGroup *_mixers; + + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main(); + + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + + int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); + void subscribe(); + + struct GPIOConfig { + uint32_t input; + uint32_t output; + uint32_t alt; + }; + + static const GPIOConfig _gpio_tab[]; + static const unsigned _ngpio; + + void gpio_reset(void); + void gpio_set_function(uint32_t gpios, int function); + void gpio_write(uint32_t gpios, int function); + uint32_t gpio_read(void); + int gpio_ioctl(device::file_t *filp, int cmd, unsigned long arg); + +}; + +namespace +{ + +PWMSim *g_pwm_sim; + +} // namespace + +bool PWMSim::_armed = false; + +PWMSim::PWMSim() : +#ifdef __PX4_NUTTX + CDev +#else + VDev +#endif + ("pwm_out_sim", PWM_OUTPUT0_DEVICE_PATH), + _task(-1), + _mode(MODE_NONE), + _update_rate(50), + _current_update_rate(0), + _control_subs { -1}, + _poll_fds_num(0), + _armed_sub(-1), + _outputs_pub(0), + _num_outputs(0), + _primary_pwm_device(false), + _groups_required(0), + _groups_subscribed(0), + _task_should_exit(false), + _mixers(nullptr) +{ + _debug_enabled = true; + memset(_controls, 0, sizeof(_controls)); + + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); +} + +PWMSim::~PWMSim() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + px4_task_delete(_task); + break; + } + + } while (_task != -1); + } + + g_pwm_sim = nullptr; +} + +int +PWMSim::init() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ +#ifdef __PX4_NUTTX + ret = CDev::init(); +#else + ret = VDev::init(); +#endif + + if (ret != OK) { + return ret; + + } else { + _primary_pwm_device = true; + } + + /* start the PWMSim interface task */ + _task = px4_task_spawn_cmd("pwm_out_sim", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1000, + (px4_main_t)&PWMSim::task_main_trampoline, + nullptr); + + if (_task < 0) { + DEVICE_DEBUG("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +PWMSim::task_main_trampoline(int argc, char *argv[]) +{ + g_pwm_sim->task_main(); +} + +int +PWMSim::set_mode(Mode mode) +{ + /* + * Configure for PWM output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { + case MODE_2PWM: + DEVICE_DEBUG("MODE_2PWM"); + /* multi-port with flow control lines as PWM */ + _update_rate = 50; /* default output rate */ + _num_outputs = 2; + break; + + case MODE_4PWM: + DEVICE_DEBUG("MODE_4PWM"); + /* multi-port as 4 PWM outs */ + _update_rate = 50; /* default output rate */ + _num_outputs = 4; + break; + + case MODE_8PWM: + DEVICE_DEBUG("MODE_8PWM"); + /* multi-port as 8 PWM outs */ + _update_rate = 50; /* default output rate */ + _num_outputs = 8; + break; + + case MODE_12PWM: + DEVICE_DEBUG("MODE_12PWM"); + /* multi-port as 12 PWM outs */ + _update_rate = 50; /* default output rate */ + _num_outputs = 12; + break; + + case MODE_16PWM: + DEVICE_DEBUG("MODE_16PWM"); + /* multi-port as 16 PWM outs */ + _update_rate = 50; /* default output rate */ + _num_outputs = 16; + break; + + case MODE_NONE: + DEVICE_DEBUG("MODE_NONE"); + /* disable servo outputs and set a very low update rate */ + _update_rate = 10; + _num_outputs = 0; + break; + + default: + return -EINVAL; + } + + _mode = mode; + return OK; +} + +int +PWMSim::set_pwm_rate(unsigned rate) +{ + if ((rate > 500) || (rate < 10)) { + return -EINVAL; + } + + _update_rate = rate; + return OK; +} + +void +PWMSim::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + PX4_DEBUG("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + + if (unsub_groups & (1 << i)) { + PX4_DEBUG("unsubscribe from actuator_controls_%d", i); + px4_close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + printf("valid\n"); + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + +void +PWMSim::task_main() +{ + /* force a reset of the update rate */ + _current_update_rate = 0; + + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_armed_sub, 200); /* 5Hz update rate */ + + /* advertise the mixed control outputs */ + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + + /* advertise the mixed control outputs, insist on the first group output */ + _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); + + + DEVICE_LOG("starting"); + + /* loop until killed */ + while (!_task_should_exit) { + + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + } + + /* handle update rate changes */ + if (_current_update_rate != _update_rate) { + int update_rate_in_ms = int(1000 / _update_rate); + + if (update_rate_in_ms < 2) { + update_rate_in_ms = 2; + } + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); + } + } + + // up_pwm_servo_set_rate(_update_rate); + _current_update_rate = _update_rate; + } + + /* sleep waiting for data, but no more than a second */ + int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); + + /* this would be bad... */ + if (ret < 0) { + DEVICE_LOG("poll error %d", errno); + continue; + } + + if (ret == 0) { + // timeout + continue; + } + + /* get controls for required topics */ + unsigned poll_id = 0; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + } + + poll_id++; + } + } + + /* can we mix? */ + if (_armed && _mixers != nullptr) { + + size_t num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_6PWM: + num_outputs = 6; + break; + + case MODE_8PWM: + num_outputs = 8; + break; + + default: + num_outputs = 0; + break; + } + + /* do mixing */ + actuator_outputs_s outputs; + num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); + outputs.timestamp = hrt_absolute_time(); + + /* disable unused ports by setting their output to NaN */ + for (size_t i = 0; i < sizeof(outputs.output) / sizeof(outputs.output[0]); i++) { + if (i >= num_outputs) { + outputs.output[i] = NAN; + } + } + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + PX4_ISFINITE(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + + } else { + /* + * 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] = 900; + } + } + + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); + } + + /* how about an arming update? */ + bool updated; + actuator_armed_s aa; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa); + /* do not obey the lockdown value, as lockdown is for PWMSim */ + _armed = aa.armed; + } + } + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + px4_close(_control_subs[i]); + } + } + + px4_close(_armed_sub); + + /* make sure servos are off */ + // up_pwm_servo_deinit(); + + /* note - someone else is responsible for restoring the GPIO config */ + + /* tell the dtor that we are exiting */ + _task = -1; +} + +int +PWMSim::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + if (_armed) { + input = controls[control_group].control[control_index]; + + } else { + /* clamp actuator to zero if not armed */ + input = 0.0f; + } + + return 0; +} + +int +PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + int ret; + + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + switch (_mode) { + case MODE_2PWM: + case MODE_4PWM: + case MODE_8PWM: + case MODE_12PWM: + case MODE_16PWM: + ret = PWMSim::pwm_ioctl(filp, cmd, arg); + break; + + default: + ret = -ENOTTY; + DEVICE_DEBUG("not in a PWM mode"); + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) { +#ifdef __PX4_NUTTX + ret = CDev::ioctl(filp, cmd, arg); +#else + ret = VDev::ioctl(filp, cmd, arg); +#endif + } + + return ret; +} + +int +PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + int ret = OK; + // int channel; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + // up_pwm_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + // up_pwm_servo_arm(false); + break; + + case PWM_SERVO_SET_UPDATE_RATE: + // PWMSim always outputs at the alternate (usually faster) rate + g_pwm_sim->set_pwm_rate(arg); + break; + + case PWM_SERVO_SET_SELECT_UPDATE_RATE: + // PWMSim always outputs at the alternate (usually faster) rate + break; + + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + *(uint32_t *)arg = 400; + break; + + case PWM_SERVO_GET_UPDATE_RATE: + *(uint32_t *)arg = 400; + break; + + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + *(uint32_t *)arg = 0; + break; + + case PWM_SERVO_GET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 850; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 900; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_MIN_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 1000; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_MAX_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 2000; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_SET(2): + case PWM_SERVO_SET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_SET(0): + case PWM_SERVO_SET(1): + if (arg < 2100) { + // channel = cmd - PWM_SERVO_SET(0); +// up_pwm_servo_set(channel, arg); XXX + + } else { + ret = -EINVAL; + } + + break; + + case PWM_SERVO_GET(7): + case PWM_SERVO_GET(6): + case PWM_SERVO_GET(5): + case PWM_SERVO_GET(4): + if (_num_outputs < 8) { + ret = -EINVAL; + break; + } + + case PWM_SERVO_GET(3): + case PWM_SERVO_GET(2): + if (_num_outputs < 4) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_GET(1): + case PWM_SERVO_GET(0): { + *(servo_position_t *)arg = 1500; + break; + } + + case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { + // no restrictions on output grouping + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + + *(uint32_t *)arg = (1 << channel); + break; + } + + case PWM_SERVO_GET_COUNT: + case MIXERIOCGETOUTPUTCOUNT: + if (_mode == MODE_8PWM) { + *(unsigned *)arg = 8; + + } else if (_mode == MODE_4PWM) { + + *(unsigned *)arg = 4; + + } else { + + *(unsigned *)arg = 2; + } + + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + } + + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + _groups_required = 0; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + _mixers->add_mixer(mixer); + _mixers->groups_required(_groups_required); + } + + break; + } + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) { + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + } + + if (_mixers == nullptr) { + _groups_required = 0; + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + DEVICE_DEBUG("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + ret = -EINVAL; + + } else { + _mixers->groups_required(_groups_required); + } + } + + break; + } + + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +namespace +{ + +enum PortMode { + PORT_MODE_UNDEFINED = 0, + PORT1_MODE_UNSET, + PORT1_FULL_PWM, + PORT1_PWM_AND_SERIAL, + PORT1_PWM_AND_GPIO, + PORT2_MODE_UNSET, + PORT2_8PWM, + PORT2_12PWM, + PORT2_16PWM, +}; + +static PortMode g_port_mode = PORT_MODE_UNDEFINED; + +int +hil_new_mode(PortMode new_mode) +{ + // uint32_t gpio_bits; + + +// /* reset to all-inputs */ +// g_pwm_sim->ioctl(0, GPIO_RESET, 0); + + // gpio_bits = 0; + + PWMSim::Mode servo_mode = PWMSim::MODE_NONE; + + switch (new_mode) { + case PORT_MODE_UNDEFINED: + case PORT1_MODE_UNSET: + case PORT2_MODE_UNSET: + /* nothing more to do here */ + break; + + case PORT1_FULL_PWM: + /* select 4-pin PWM mode */ + servo_mode = PWMSim::MODE_8PWM; + break; + + case PORT1_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = PWMSim::MODE_2PWM; +// /* set RX/TX multi-GPIOs to serial mode */ +// gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT1_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = PWMSim::MODE_2PWM; + break; + + case PORT2_8PWM: + /* select 8-pin PWM mode */ + servo_mode = PWMSim::MODE_8PWM; + break; + + case PORT2_12PWM: + /* select 12-pin PWM mode */ + servo_mode = PWMSim::MODE_12PWM; + break; + + case PORT2_16PWM: + /* select 16-pin PWM mode */ + servo_mode = PWMSim::MODE_16PWM; + break; + } + +// /* adjust GPIO config for serial mode(s) */ +// if (gpio_bits != 0) +// g_pwm_sim->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + + /* (re)set the PWM output mode */ + g_pwm_sim->set_mode(servo_mode); + + return OK; +} + +int +test(void) +{ + int fd; + + fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); + + if (fd < 0) { + puts("open fail"); + return -ENODEV; + } + + px4_ioctl(fd, PWM_SERVO_ARM, 0); + px4_ioctl(fd, PWM_SERVO_SET(0), 1000); + + px4_close(fd); + + return OK; +} + +int +fake(int argc, char *argv[]) +{ + if (argc < 5) { + puts("pwm_out_sim fake (values -100 .. 100)"); + return -EINVAL; + } + + actuator_controls_s ac; + + ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; + + ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; + + ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; + + ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; + + orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); + + if (handle == nullptr) { + puts("advertise failed"); + return 1; + } + + return 0; +} + +} // namespace + +extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[]); + +static void +usage() +{ + PX4_WARN("pwm_out_sim: unrecognized command, try:"); + PX4_WARN(" mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16"); +} + +int +pwm_out_sim_main(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNDEFINED; + const char *verb; + int ret = OK; + + if (argc < 2) { + usage(); + return -EINVAL; + } + + verb = argv[1]; + + if (g_pwm_sim == nullptr) { + g_pwm_sim = new PWMSim; + + if (g_pwm_sim == nullptr) { + return -ENOMEM; + } + } + + /* + * Mode switches. + */ + + // this was all cut-and-pasted from the FMU driver; it's junk + if (!strcmp(verb, "mode_pwm")) { + new_mode = PORT1_FULL_PWM; + + } else if (!strcmp(verb, "mode_pwm_serial")) { + new_mode = PORT1_PWM_AND_SERIAL; + + } else if (!strcmp(verb, "mode_pwm_gpio")) { + new_mode = PORT1_PWM_AND_GPIO; + + } else if (!strcmp(verb, "mode_port2_pwm8")) { + new_mode = PORT2_8PWM; + + } else if (!strcmp(verb, "mode_port2_pwm12")) { + new_mode = PORT2_8PWM; + + } else if (!strcmp(verb, "mode_port2_pwm16")) { + new_mode = PORT2_8PWM; + } + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNDEFINED) { + + /* yes but it's the same mode */ + if (new_mode == g_port_mode) { + return OK; + } + + /* switch modes */ + ret = hil_new_mode(new_mode); + + } else if (!strcmp(verb, "test")) { + ret = test(); + } + + else if (!strcmp(verb, "fake")) { + ret = fake(argc - 1, argv + 1); + } + + else { + usage(); + ret = -EINVAL; + } + + if (ret == OK && g_pwm_sim->_task == -1) { + ret = g_pwm_sim->init(); + + if (ret != OK) { + warnx("failed to start the pwm_out_sim driver"); + delete g_pwm_sim; + g_pwm_sim = nullptr; + } + } + + return ret; +} diff --git a/src/drivers/px4io/CMakeLists.txt b/src/drivers/px4io/CMakeLists.txt new file mode 100644 index 000000000000..5c180c6f8220 --- /dev/null +++ b/src/drivers/px4io/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__px4io + MAIN px4io + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + px4io.cpp + px4io_uploader.cpp + px4io_serial.cpp + px4io_i2c.cpp + px4io_params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/px4io/px4io_driver.h b/src/drivers/px4io/px4io_driver.h new file mode 100644 index 000000000000..591c3920d2b0 --- /dev/null +++ b/src/drivers/px4io/px4io_driver.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4io_driver.h + * + * Interface for PX4IO + */ + +#pragma once + +#include + +#ifdef PX4_I2C_OBDEV_PX4IO +device::Device *PX4IO_i2c_interface(); +#endif + +#ifdef PX4IO_SERIAL_BASE +device::Device *PX4IO_serial_interface(); +#endif diff --git a/src/drivers/rgbled/CMakeLists.txt b/src/drivers/rgbled/CMakeLists.txt new file mode 100644 index 000000000000..d833567467e5 --- /dev/null +++ b/src/drivers/rgbled/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__rgbled + MAIN rgbled + COMPILE_FLAGS + -Os + SRCS + rgbled.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp new file mode 100644 index 000000000000..e3f6ad7d08e3 --- /dev/null +++ b/src/drivers/srf02/srf02.cpp @@ -0,0 +1,959 @@ +/**************************************************************************** + * + * 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 srf02.cpp + * + * Driver for the SRF02 sonar range finder adapted from the Maxbotix sonar range finder driver (mb12xx). + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +/* Configuration Constants */ +#define SRF02_BUS PX4_I2C_BUS_EXPANSION +#define SRF02_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ +#define SRF02_DEVICE_PATH "/dev/srf02" + +/* MB12xx Registers addresses */ + +#define SRF02_TAKE_RANGE_REG 0x51 /* Measure range Register */ +#define SRF02_SET_ADDRESS_0 0xA0 /* Change address 0 Register */ +#define SRF02_SET_ADDRESS_1 0xAA /* Change address 1 Register */ +#define SRF02_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ + +/* Device limits */ +#define SRF02_MIN_DISTANCE (0.20f) +#define SRF02_MAX_DISTANCE (7.65f) + +#define SRF02_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ +#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ + + +/* 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 SRF02 : public device::I2C +{ +public: + SRF02(int bus = SRF02_BUS, int address = SRF02_BASEADDR); + virtual ~SRF02(); + + 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::RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + int _orb_class_instance; + + orb_advert_t _distance_sensor_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ + std::vector addr_ind; /* temp sonar i2c address vector */ + std::vector + _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @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 SRF02_MIN_DISTANCE + * and SRF02_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); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int srf02_main(int argc, char *argv[]); + +SRF02::SRF02(int bus, int address) : + I2C("MB12xx", SRF02_DEVICE_PATH, bus, address, 100000), + _min_distance(SRF02_MIN_DISTANCE), + _max_distance(SRF02_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _class_instance(-1), + _orb_class_instance(-1), + _distance_sensor_topic(nullptr), + _sample_perf(perf_alloc(PC_ELAPSED, "srf02_read")), + _comms_errors(perf_alloc(PC_COUNT, "srf02_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "srf02_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 */ + _debug_enabled = false; + + /* work_cancel in the dtor will explode if we don't do this... */ + memset(&_work, 0, sizeof(_work)); +} + +SRF02::~SRF02() +{ + /* 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_BASE_DEVICE_PATH, _class_instance); + } + + /* free perf counters */ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +SRF02::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + return ret; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); + + _index_counter = SRF02_BASEADDR; /* set temp sonar i2c address to base adress */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + + if (_reports == nullptr) { + return ret; + } + + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report = {}; + + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); + + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor 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 = SRF02_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); + DEVICE_DEBUG("sonar added"); + _latest_sonar_measurements.push_back(200); + } + } + + _index_counter = SRF02_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 = SRF02_CONVERSION_INTERVAL; + + } else { + _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES; + } + + /* show the connected sonars in terminal */ + for (unsigned i = 0; i < addr_ind.size(); i++) { + DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]); + } + + DEVICE_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; + + return ret; +} + +int +SRF02::probe() +{ + return measure(); +} + +void +SRF02::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +SRF02::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +SRF02::get_minimum_distance() +{ + return _min_distance; +} + +float +SRF02::get_maximum_distance() +{ + return _max_distance; +} + +int +SRF02::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(_cycling_rate); + + /* 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 */ + int ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(_cycling_rate)) { + 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 +SRF02::read(struct file *filp, char *buffer, size_t buflen) +{ + + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *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(_cycling_rate * 2); + + /* 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 +SRF02::measure() +{ + + int ret; + + /* + * Send the command to begin a measurement. + */ + + uint8_t cmd[2]; + cmd[0] = 0x00; + cmd[1] = SRF02_TAKE_RANGE_REG; + ret = transfer(cmd, 2, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + DEVICE_DEBUG("i2c::transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +SRF02::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + uint8_t cmd = 0x02; + perf_begin(_sample_perf); + + ret = transfer(&cmd, 1, nullptr, 0); + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) { + DEVICE_DEBUG("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t distance_cm = val[0] << 8 | val[1]; + float distance_m = float(distance_cm) * 1e-2f; + + struct distance_sensor_s report; + report.timestamp = hrt_absolute_time(); + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + report.orientation = 8; + report.current_distance = distance_m; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0f; + /* TODO: set proper ID */ + report.id = 0; + + /* publish it, if we are the primary */ + if (_distance_sensor_topic != nullptr) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_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 +SRF02::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)&SRF02::cycle_trampoline, this, 5); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = nullptr; + + if (pub != nullptr) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + + } +} + +void +SRF02::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +SRF02::cycle_trampoline(void *arg) +{ + + SRF02 *dev = (SRF02 *)arg; + + dev->cycle(); + +} + +void +SRF02::cycle() +{ + 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()) { + DEVICE_DEBUG("collection error"); + /* if error restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* 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)&SRF02::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(_cycling_rate)); + return; + } + } + + /* 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()) { + DEVICE_DEBUG("measure error sonar adress %d", _index_counter); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&SRF02::cycle_trampoline, + this, + USEC2TICK(_cycling_rate)); + +} + +void +SRF02::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 srf02 +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +SRF02 *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 SRF02(SRF02_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(SRF02_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 distance_sensor_s report; + ssize_t sz; + int ret; + + int fd = open(SRF02_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'srf02 start' if the driver is not running", SRF02_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.current_distance); + warnx("time: %llu", 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 5x and report each value */ + for (unsigned i = 0; i < 5; 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", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + warnx("measurement: %0.3f", (double)report.current_distance); + warnx("time: %llu", 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(SRF02_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 +srf02_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + srf02::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + srf02::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + srf02::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + srf02::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + srf02::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/stm32/CMakeLists.txt b/src/drivers/stm32/CMakeLists.txt new file mode 100644 index 000000000000..e81bf502770f --- /dev/null +++ b/src/drivers/stm32/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__stm32 + COMPILE_FLAGS + -Os + SRCS + drv_hrt.c + drv_pwm_servo.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/examples/fixedwing_control/CMakeLists.txt b/src/examples/fixedwing_control/CMakeLists.txt new file mode 100644 index 000000000000..e1032db30531 --- /dev/null +++ b/src/examples/fixedwing_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE examples__fixedwing_control + MAIN ex_fixedwing_control + STACK 1200 + COMPILE_FLAGS + -Wframe-larger-than=1300 + + SRCS + main.c + params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/examples/hwtest/CMakeLists.txt b/src/examples/hwtest/CMakeLists.txt new file mode 100644 index 000000000000..31548df813ac --- /dev/null +++ b/src/examples/hwtest/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE examples__hwtest + MAIN ex_hwtest + SRCS + hwtest.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/examples/matlab_csv_serial/CMakeLists.txt b/src/examples/matlab_csv_serial/CMakeLists.txt new file mode 100644 index 000000000000..516acda09280 --- /dev/null +++ b/src/examples/matlab_csv_serial/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE examples__matlab_csv_serial + MAIN matlab_csv_serial + SRCS + matlab_csv_serial.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/examples/publisher/CMakeLists.txt b/src/examples/publisher/CMakeLists.txt new file mode 100644 index 000000000000..d48aebf6c1f4 --- /dev/null +++ b/src/examples/publisher/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE examples__publisher + MAIN publisher + STACK 1200 + SRCS + publisher_main.cpp + publisher_start_nuttx.cpp + publisher_example.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/examples/px4_daemon_app/CMakeLists.txt b/src/examples/px4_daemon_app/CMakeLists.txt new file mode 100644 index 000000000000..2d455353d9cf --- /dev/null +++ b/src/examples/px4_daemon_app/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE examples__px4_daemon_app + MAIN px4_daemon_app + STACK 1200 + SRCS + px4_daemon_app.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/examples/px4_mavlink_debug/CMakeLists.txt b/src/examples/px4_mavlink_debug/CMakeLists.txt new file mode 100644 index 000000000000..2f1b5a6a2f2f --- /dev/null +++ b/src/examples/px4_mavlink_debug/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE examples__px4_mavlink_debug + MAIN px4_mavlink_debug + STACK 2000 + SRCS + px4_mavlink_debug.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/examples/px4_simple_app/CMakeLists.txt b/src/examples/px4_simple_app/CMakeLists.txt new file mode 100644 index 000000000000..5ba6781c6052 --- /dev/null +++ b/src/examples/px4_simple_app/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE examples__px4_simple_app + MAIN px4_simple_app + SRCS + px4_simple_app.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/examples/rover_steering_control/CMakeLists.txt b/src/examples/rover_steering_control/CMakeLists.txt new file mode 100644 index 000000000000..0ba4b03f529f --- /dev/null +++ b/src/examples/rover_steering_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE examples__rover_steering_control + MAIN rover_steering_control + STACK 1200 + COMPILE_FLAGS + -Wframe-larger-than=1300 + + SRCS + main.cpp + params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/examples/subscriber/CMakeLists.txt b/src/examples/subscriber/CMakeLists.txt new file mode 100644 index 000000000000..817d309d20b0 --- /dev/null +++ b/src/examples/subscriber/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE examples__subscriber + MAIN subscriber + STACK 2400 + SRCS + subscriber_main.cpp + subscriber_start_nuttx.cpp + subscriber_example.cpp + subscriber_params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/firmware/nuttx/gdbinit.in b/src/firmware/nuttx/gdbinit.in new file mode 100644 index 000000000000..e820673b216a --- /dev/null +++ b/src/firmware/nuttx/gdbinit.in @@ -0,0 +1,7 @@ +target extended ${DEBUG_PORT} +monitor swdp_scan +attach 1 +monitor vector_catch disable hard +set mem inaccessible-by-default off +set print pretty +source ${CMAKE_SOURCE_DIR}/Debug/PX4 diff --git a/src/lib/conversion/CMakeLists.txt b/src/lib/conversion/CMakeLists.txt new file mode 100644 index 000000000000..78f39e99aad0 --- /dev/null +++ b/src/lib/conversion/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__conversion + COMPILE_FLAGS + -Os + SRCS + rotation.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/ecl/CMakeLists.txt b/src/lib/ecl/CMakeLists.txt new file mode 100644 index 000000000000..9f8e02ba2e7a --- /dev/null +++ b/src/lib/ecl/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__ecl + COMPILE_FLAGS + -Os + SRCS + attitude_fw/ecl_controller.cpp + attitude_fw/ecl_pitch_controller.cpp + attitude_fw/ecl_roll_controller.cpp + attitude_fw/ecl_yaw_controller.cpp + attitude_fw/ecl_wheel_controller.cpp + l1/ecl_l1_pos_controller.cpp + validation/data_validator.cpp + validation/data_validator_group.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp new file mode 100644 index 000000000000..7e34ff2a61f2 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_wheel_controller.cpp + * Implementation of a simple PID wheel controller for heading tracking. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_wheel_controller.h" +#include +#include +#include +#include +#include +#include +#include + +ECL_WheelController::ECL_WheelController() : + ECL_Controller("wheel") +{ +} + +ECL_WheelController::~ECL_WheelController() +{ +} + +float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.groundspeed) && + PX4_ISFINITE(ctl_data.groundspeed_scaler))) { + perf_count(_nonfinite_input_perf); + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; + + if (dt_micros > 500000) { + lock_integrator = true; + } + + /* input conditioning */ + float min_speed = 1.0f; + + /* Calculate body angular rate error */ + _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error + + if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { + + float id = _rate_error * dt * ctl_data.groundspeed_scaler; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + + _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + + integrator_constrained; + /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f", + (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/ + + + return math::constrain(_last_output, -1.0f, 1.0f); +} + + +float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_ISFINITE(ctl_data.yaw))) { + perf_count(_nonfinite_input_perf); + warnx("not controlling wheel"); + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + /* shortest angle (wrap around) */ + yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F; + /*warnx("yaw_error: %.4f", (double)yaw_error);*/ + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + + return _rate_setpoint; +} diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.h b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h new file mode 100644 index 000000000000..f153a8f8f189 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_wheel_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Andreas Antener + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ +#ifndef ECL_HEADING_CONTROLLER_H +#define ECL_HEADING_CONTROLLER_H + +#include +#include + +#include "ecl_controller.h" + +class __EXPORT ECL_WheelController : + public ECL_Controller +{ +public: + ECL_WheelController(); + + ~ECL_WheelController(); + + float control_attitude(const struct ECL_ControlData &ctl_data); + + float control_bodyrate(const struct ECL_ControlData &ctl_data); +}; + +#endif // ECL_HEADING_CONTROLLER_H diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp new file mode 100644 index 000000000000..299811ea435a --- /dev/null +++ b/src/lib/ecl/validation/data_validator.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator.c + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#include "data_validator.h" +#include + +DataValidator::DataValidator(DataValidator *prev_sibling) : + _error_mask(ERROR_FLAG_NO_ERROR), + _time_last(0), + _timeout_interval(20000), + _event_count(0), + _error_count(0), + _error_density(0), + _priority(0), + _mean{0.0f}, + _lp{0.0f}, + _M2{0.0f}, + _rms{0.0f}, + _value{0.0f}, + _value_equal_count(0), + _sibling(prev_sibling) +{ + +} + +DataValidator::~DataValidator() +{ + +} + +void +DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in) +{ + _event_count++; + + if (error_count_in > _error_count) { + _error_density += (error_count_in - _error_count); + } else if (_error_density > 0) { + _error_density--; + } + + _error_count = error_count_in; + _priority = priority_in; + + for (unsigned i = 0; i < _dimensions; i++) { + if (_time_last == 0) { + _mean[i] = 0; + _lp[i] = val[i]; + _M2[i] = 0; + } else { + float lp_val = val[i] - _lp[i]; + + float delta_val = lp_val - _mean[i]; + _mean[i] += delta_val / _event_count; + _M2[i] += delta_val * (lp_val - _mean[i]); + _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); + + if (fabsf(_value[i] - val[i]) < 0.000001f) { + _value_equal_count++; + } else { + _value_equal_count = 0; + } + } + + // XXX replace with better filter, make it auto-tune to update rate + _lp[i] = _lp[i] * 0.5f + val[i] * 0.5f; + + _value[i] = val[i]; + } + + _time_last = timestamp; +} + +float +DataValidator::confidence(uint64_t timestamp) +{ + float ret = 1.0f; + + /* check if we have any data */ + if (_time_last == 0) { + _error_mask |= ERROR_FLAG_NO_DATA; + ret = 0.0f; + } + + /* timed out - that's it */ + if (timestamp - _time_last > _timeout_interval) { + _error_mask |= ERROR_FLAG_TIMEOUT; + ret = 0.0f; + } + + /* we got the exact same sensor value N times in a row */ + if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { + _error_mask |= ERROR_FLAG_STALE_DATA; + ret = 0.0f; + } + + /* check error count limit */ + if (_error_count > NORETURN_ERRCOUNT) { + _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; + ret = 0.0f; + } + + /* cap error density counter at window size */ + if (_error_density > ERROR_DENSITY_WINDOW) { + _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; + _error_density = ERROR_DENSITY_WINDOW; + } + + /* no critical errors */ + if(ret > 0.0f) { + /* return local error density for last N measurements */ + ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); + } + + return ret; +} + +void +DataValidator::print() +{ + if (_time_last == 0) { + ECL_INFO("\tno data"); + return; + } + + for (unsigned i = 0; i < _dimensions; i++) { + ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f", + (double) _value[i], (double)_lp[i], (double)_mean[i], + (double)_rms[i], (double)confidence(hrt_absolute_time())); + } +} diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h new file mode 100644 index 000000000000..31f6d9d98998 --- /dev/null +++ b/src/lib/ecl/validation/data_validator.h @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator.h + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#pragma once + +#include +#include + +class __EXPORT DataValidator { +public: + DataValidator(DataValidator *prev_sibling = nullptr); + virtual ~DataValidator(); + + /** + * Put an item into the validator. + * + * @param val Item to put + */ + void put(uint64_t timestamp, float val[3], uint64_t error_count, int priority); + + /** + * Get the next sibling in the group + * + * @return the next sibling + */ + DataValidator* sibling() { return _sibling; } + + /** + * Get the confidence of this validator + * @return the confidence between 0 and 1 + */ + float confidence(uint64_t timestamp); + + /** + * Get the error count of this validator + * @return the error count + */ + uint64_t error_count() { return _error_count; } + + /** + * Get the values of this validator + * @return the stored value + */ + float* value() { return _value; } + + /** + * Get the used status of this validator + * @return true if this validator ever saw data + */ + bool used() { return (_time_last > 0); } + + /** + * Get the priority of this validator + * @return the stored priority + */ + int priority() { return (_priority); } + + /** + * Get the error state of this validator + * @return the bitmask with the error status + */ + uint32_t state() { return (_error_mask); } + + /** + * Reset the error state of this validator + */ + void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } + + /** + * Get the RMS values of this validator + * @return the stored RMS + */ + float* rms() { return _rms; } + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } + + /** + * Data validator error states + */ + static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U); + static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U); + static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1); + static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); + +private: + static const unsigned _dimensions = 3; + uint32_t _error_mask; /**< sensor error state */ + uint64_t _time_last; /**< last timestamp */ + uint64_t _timeout_interval; /**< interval in which the datastream times out in us */ + uint64_t _event_count; /**< total data counter */ + uint64_t _error_count; /**< error count */ + int _error_density; /**< ratio between successful reads and errors */ + int _priority; /**< sensor nominal priority */ + float _mean[_dimensions]; /**< mean of value */ + float _lp[3]; /**< low pass value */ + float _M2[3]; /**< RMS component value */ + float _rms[3]; /**< root mean square error */ + float _value[3]; /**< last value */ + float _value_equal_count; /**< equal values in a row */ + DataValidator *_sibling; /**< sibling in the group */ + const unsigned NORETURN_ERRCOUNT = 10000; /**< if the error count reaches this value, return sensor as invalid */ + const float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */ + const unsigned VALUE_EQUAL_COUNT_MAX = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */ + + /* we don't want this class to be copied */ + DataValidator(const DataValidator&); + DataValidator operator=(const DataValidator&); +}; diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp new file mode 100644 index 000000000000..8bf060cf653d --- /dev/null +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator_group.cpp + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#include "data_validator_group.h" +#include + +DataValidatorGroup::DataValidatorGroup(unsigned siblings) : + _first(nullptr), + _curr_best(-1), + _prev_best(-1), + _first_failover_time(0), + _toggle_count(0) +{ + DataValidator *next = _first; + + for (unsigned i = 0; i < siblings; i++) { + next = new DataValidator(next); + } + + _first = next; +} + +DataValidatorGroup::~DataValidatorGroup() +{ + +} + +void +DataValidatorGroup::set_timeout(uint64_t timeout_interval_us) +{ + DataValidator *next = _first; + + while (next != nullptr) { + next->set_timeout(timeout_interval_us); + next = next->sibling(); + } +} + +void +DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority) +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (i == index) { + next->put(timestamp, val, error_count, priority); + break; + } + next = next->sibling(); + i++; + } +} + +float* +DataValidatorGroup::get_best(uint64_t timestamp, int *index) +{ + DataValidator *next = _first; + + // XXX This should eventually also include voting + int pre_check_best = _curr_best; + float pre_check_confidence = 1.0f; + int pre_check_prio = -1; + float max_confidence = -1.0f; + int max_priority = -1000; + int max_index = -1; + DataValidator *best = nullptr; + + unsigned i = 0; + + while (next != nullptr) { + float confidence = next->confidence(timestamp); + + if (static_cast(i) == pre_check_best) { + pre_check_prio = next->priority(); + pre_check_confidence = confidence; + } + + /* + * Switch if: + * 1) the confidence is higher and priority is equal or higher + * 2) the confidence is no less than 1% different and the priority is higher + */ + if (((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) || + (confidence > max_confidence && (next->priority() >= max_priority)) || + (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority)) + ) { + max_index = i; + max_confidence = confidence; + max_priority = next->priority(); + best = next; + } + + next = next->sibling(); + i++; + } + + /* the current best sensor is not matching the previous best sensor */ + if (max_index != _curr_best) { + + bool true_failsafe = true; + + /* check whether the switch was a failsafe or preferring a higher priority sensor */ + if (pre_check_prio != -1 && pre_check_prio < max_priority && + fabsf(pre_check_confidence - max_confidence) < 0.1f) { + /* this is not a failover */ + true_failsafe = false; + /* reset error flags, this is likely a hotplug sensor coming online late */ + best->reset_state(); + } + + /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ + if (_curr_best < 0) { + _prev_best = max_index; + } else { + /* we were initialized before, this is a real failsafe */ + _prev_best = pre_check_best; + + if (true_failsafe) { + _toggle_count++; + + /* if this is the first time, log when we failed */ + if (_first_failover_time == 0) { + _first_failover_time = timestamp; + } + } + } + + /* for all cases we want to keep a record of the best index */ + _curr_best = max_index; + } + *index = max_index; + return (best) ? best->value() : nullptr; +} + +float +DataValidatorGroup::get_vibration_factor(uint64_t timestamp) +{ + DataValidator *next = _first; + + float vibe = 0.0f; + + /* find the best RMS value of a non-timed out sensor */ + while (next != nullptr) { + + if (next->confidence(timestamp) > 0.5f) { + float* rms = next->rms(); + + for (unsigned j = 0; j < 3; j++) { + if (rms[j] > vibe) { + vibe = rms[j]; + } + } + } + + next = next->sibling(); + } + + return vibe; +} + +void +DataValidatorGroup::print() +{ + /* print the group's state */ + ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", + _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", + _toggle_count); + + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used()) { + uint32_t flags = next->state(); + + ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""), + ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); + + next->print(); + } + next = next->sibling(); + i++; + } +} + +unsigned +DataValidatorGroup::failover_count() +{ + return _toggle_count; +} + +int +DataValidatorGroup::failover_index() +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { + return i; + } + next = next->sibling(); + i++; + } + return -1; +} + +uint32_t +DataValidatorGroup::failover_state() +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { + return next->state(); + } + next = next->sibling(); + i++; + } + return DataValidator::ERROR_FLAG_NO_ERROR; +} diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h new file mode 100644 index 000000000000..855c3ed4aecf --- /dev/null +++ b/src/lib/ecl/validation/data_validator_group.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator_group.h + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#pragma once + +#include "data_validator.h" + +class __EXPORT DataValidatorGroup { +public: + DataValidatorGroup(unsigned siblings); + virtual ~DataValidatorGroup(); + + /** + * Put an item into the validator group. + * + * @param index Sensor index + * @param timestamp The timestamp of the measurement + * @param val The 3D vector + * @param error_count The current error count of the sensor + * @param priority The priority of the sensor + */ + void put(unsigned index, uint64_t timestamp, + float val[3], uint64_t error_count, int priority); + + /** + * Get the best data triplet of the group + * + * @return pointer to the array of best values + */ + float* get_best(uint64_t timestamp, int *index); + + /** + * Get the RMS / vibration factor + * + * @return float value representing the RMS, which a valid indicator for vibration + */ + float get_vibration_factor(uint64_t timestamp); + + /** + * Get the number of failover events + * + * @return the number of failovers + */ + unsigned failover_count(); + + /** + * Get the index of the failed sensor in the group + * + * @return index of the failed sensor + */ + int failover_index(); + + /** + * Get the error state of the failed sensor in the group + * + * @return bitmask with erro states of the failed sensor + */ + uint32_t failover_state(); + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint64_t timeout_interval_us); + +private: + DataValidator *_first; /**< sibling in the group */ + int _curr_best; /**< currently best index */ + int _prev_best; /**< the previous best index */ + uint64_t _first_failover_time; /**< timestamp where the first failover occured or zero if none occured */ + unsigned _toggle_count; /**< number of back and forth switches between two sensors */ + static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; + + /* we don't want this class to be copied */ + DataValidatorGroup(const DataValidatorGroup&); + DataValidatorGroup operator=(const DataValidatorGroup&); +}; diff --git a/src/lib/external_lgpl/CMakeLists.txt b/src/lib/external_lgpl/CMakeLists.txt new file mode 100644 index 000000000000..b46af5006d46 --- /dev/null +++ b/src/lib/external_lgpl/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__external_lgpl + COMPILE_FLAGS + -Os + SRCS + tecs/tecs.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/geo/CMakeLists.txt b/src/lib/geo/CMakeLists.txt new file mode 100644 index 000000000000..14b80ef2a565 --- /dev/null +++ b/src/lib/geo/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__geo + SRCS + geo.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/geo_lookup/CMakeLists.txt b/src/lib/geo_lookup/CMakeLists.txt new file mode 100644 index 000000000000..ea13af025d67 --- /dev/null +++ b/src/lib/geo_lookup/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__geo_lookup + COMPILE_FLAGS + -Os + SRCS + geo_mag_declination.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/launchdetection/CMakeLists.txt b/src/lib/launchdetection/CMakeLists.txt new file mode 100644 index 000000000000..b396169a450f --- /dev/null +++ b/src/lib/launchdetection/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__launchdetection + COMPILE_FLAGS + -Os + SRCS + LaunchDetector.cpp + CatapultLaunchMethod.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt new file mode 100644 index 000000000000..6bfe964d79f3 --- /dev/null +++ b/src/lib/mathlib/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__mathlib + SRCS + math/test/test.cpp + math/Limits.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/mathlib/math/filter/CMakeLists.txt b/src/lib/mathlib/math/filter/CMakeLists.txt new file mode 100644 index 000000000000..785b2f2c6c6d --- /dev/null +++ b/src/lib/mathlib/math/filter/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__mathlib__math__filter + SRCS + LowPassFilter2p.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp new file mode 100644 index 000000000000..708988985845 --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -0,0 +1,286 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file RunwayTakeoff.cpp + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#include +#include +#include + +#include "RunwayTakeoff.h" +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +RunwayTakeoff::RunwayTakeoff() : + SuperBlock(NULL, "RWTO"), + _state(), + _initialized(false), + _initialized_time(0), + _init_yaw(0), + _climbout(false), + _throttle_ramp_time(2 * 1e6), + _start_wp(), + _runway_takeoff_enabled(this, "TKOFF"), + _heading_mode(this, "HDG"), + _nav_alt(this, "NAV_ALT"), + _takeoff_throttle(this, "MAX_THR"), + _runway_pitch_sp(this, "PSP"), + _max_takeoff_pitch(this, "MAX_PITCH"), + _max_takeoff_roll(this, "MAX_ROLL"), + _min_airspeed_scaling(this, "AIRSPD_SCL"), + _airspeed_min(this, "FW_AIRSPD_MIN", false), + _climbout_diff(this, "FW_CLMBOUT_DIFF", false) +{ + + updateParams(); +} + +RunwayTakeoff::~RunwayTakeoff() +{ + +} + +void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) +{ + _init_yaw = yaw; + _initialized = true; + _state = RunwayTakeoffState::THROTTLE_RAMP; + _initialized_time = hrt_absolute_time(); + _climbout = true; // this is true until climbout is finished + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; +} + +void RunwayTakeoff::update(float airspeed, float alt_agl, + double current_lat, double current_lon, int mavlink_fd) +{ + + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: + if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) { + _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; + } + + break; + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) { + _state = RunwayTakeoffState::TAKEOFF; + mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); + } + + break; + + case RunwayTakeoffState::TAKEOFF: + if (alt_agl > _nav_alt.get()) { + _state = RunwayTakeoffState::CLIMBOUT; + + /* + * If we started in heading hold mode, move the navigation start WP to the current location now. + * The navigator will take this as starting point to navigate towards the takeoff WP. + */ + if (_heading_mode.get() == 0) { + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; + } + + mavlink_log_info(mavlink_fd, "#Climbout"); + } + + break; + + case RunwayTakeoffState::CLIMBOUT: + if (alt_agl > _climbout_diff.get()) { + _climbout = false; + _state = RunwayTakeoffState::FLY; + mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); + } + + break; + + default: + break; + } +} + +/* + * Returns true as long as we're below navigation altitude + */ +bool RunwayTakeoff::controlYaw() +{ + // keep controlling yaw directly until we start navigation + return _state < RunwayTakeoffState::CLIMBOUT; +} + +/* + * Returns pitch setpoint to use. + * + * Limited (parameter) as long as the plane is on runway. Otherwise + * use the one from TECS + */ +float RunwayTakeoff::getPitch(float tecsPitch) +{ + if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { + return math::radians(_runway_pitch_sp.get()); + } + + return tecsPitch; +} + +/* + * Returns the roll setpoint to use. + */ +float RunwayTakeoff::getRoll(float navigatorRoll) +{ + // until we have enough ground clearance, set roll to 0 + if (_state < RunwayTakeoffState::CLIMBOUT) { + return 0.0f; + } + + // allow some roll during climbout + else if (_state < RunwayTakeoffState::FLY) { + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll.get()), + math::radians(_max_takeoff_roll.get())); + } + + return navigatorRoll; +} + +/* + * Returns the yaw setpoint to use. + * + * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the + * runway. When it has enough ground clearance we start navigation towards WP. + */ +float RunwayTakeoff::getYaw(float navigatorYaw) +{ + if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) { + return _init_yaw; + + } else { + return navigatorYaw; + } +} + +/* + * Returns the throttle setpoint to use. + * + * Ramps up in the beginning, until it lifts off the runway it is set to + * parameter value, then it returns the TECS throttle. + */ +float RunwayTakeoff::getThrottle(float tecsThrottle) +{ + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: { + float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time * + _takeoff_throttle.get(); + return throttle < _takeoff_throttle.get() ? + throttle : + _takeoff_throttle.get(); + } + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + return _takeoff_throttle.get(); + + default: + return tecsThrottle; + } +} + +bool RunwayTakeoff::resetIntegrators() +{ + // reset integrators if we're still on runway + return _state < RunwayTakeoffState::TAKEOFF; +} + +/* + * Returns the minimum pitch for TECS to use. + * + * In climbout we either want what was set on the waypoint (sp_min) but at least + * the climbtout minimum pitch (parameter). + * Otherwise use the minimum that is enforced generally (parameter). + */ +float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) +{ + if (_state < RunwayTakeoffState::FLY) { + return math::max(sp_min, climbout_min); + } + + else { + return min; + } +} + +/* + * Returns the maximum pitch for TECS to use. + * + * Limited by parameter (if set) until climbout is done. + */ +float RunwayTakeoff::getMaxPitch(float max) +{ + // use max pitch from parameter if set (> 0.1) + if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) { + return _max_takeoff_pitch.get(); + } + + else { + return max; + } +} + +/* + * Returns the "previous" (start) WP for navigation. + */ +math::Vector<2> RunwayTakeoff::getStartWP() +{ + return _start_wp; +} + +void RunwayTakeoff::reset() +{ + _initialized = false; + _state = RunwayTakeoffState::THROTTLE_RAMP; +} + +} diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h new file mode 100644 index 000000000000..0e1c5ed14b8c --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file RunwayTakeoff.h + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#ifndef RUNWAYTAKEOFF_H +#define RUNWAYTAKEOFF_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +enum RunwayTakeoffState { + THROTTLE_RAMP = 0, /**< ramping up throttle */ + CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */ + TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */ + CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */ + FLY = 4 /**< fly towards takeoff waypoint */ +}; + +class __EXPORT RunwayTakeoff : public control::SuperBlock +{ +public: + RunwayTakeoff(); + ~RunwayTakeoff(); + + void init(float yaw, double current_lat, double current_lon); + void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd); + + RunwayTakeoffState getState() { return _state; }; + bool isInitialized() { return _initialized; }; + + bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; + float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; + float getInitYaw() { return _init_yaw; }; + + bool controlYaw(); + bool climbout() { return _climbout; }; + float getPitch(float tecsPitch); + float getRoll(float navigatorRoll); + float getYaw(float navigatorYaw); + float getThrottle(float tecsThrottle); + bool resetIntegrators(); + float getMinPitch(float sp_min, float climbout_min, float min); + float getMaxPitch(float max); + math::Vector<2> getStartWP(); + + void reset(); + +protected: +private: + /** state variables **/ + RunwayTakeoffState _state; + bool _initialized; + hrt_abstime _initialized_time; + float _init_yaw; + bool _climbout; + unsigned _throttle_ramp_time; + math::Vector<2> _start_wp; + + /** parameters **/ + control::BlockParamInt _runway_takeoff_enabled; + control::BlockParamInt _heading_mode; + control::BlockParamFloat _nav_alt; + control::BlockParamFloat _takeoff_throttle; + control::BlockParamFloat _runway_pitch_sp; + control::BlockParamFloat _max_takeoff_pitch; + control::BlockParamFloat _max_takeoff_roll; + control::BlockParamFloat _min_airspeed_scaling; + control::BlockParamFloat _airspeed_min; + control::BlockParamFloat _climbout_diff; + +}; + +} + +#endif // RUNWAYTAKEOFF_H diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c new file mode 100644 index 000000000000..f5a1017f967a --- /dev/null +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file runway_takeoff_params.c + * + * Parameters for runway takeoff + * + * @author Andreas Antener + */ + +#include + +#include + +/** + * Enable or disable runway takeoff with landing gear + * + * 0: disabled, 1: enabled + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_TKOFF, 0); + +/** + * Specifies which heading should be held during runnway takeoff. + * + * 0: airframe heading, 1: heading towards takeoff waypoint + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_HDG, 0); + +/** + * Altitude AGL at which we have enough ground clearance to allow some roll. + * Until RWTO_NAV_ALT is reached the plane is held level and only + * rudder is used to keep the heading (see RWTO_HDG). This should be below + * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. + * + * @unit m + * @min 0.0 + * @max 100.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); + +/** + * Max throttle during runway takeoff. + * (Can be used to test taxi on runway) + * + * @min 0.0 + * @max 1.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); + +/** + * Pitch setpoint during taxi / before takeoff airspeed is reached. + * A taildragger with stearable wheel might need to pitch up + * a little to keep it's wheel on the ground before airspeed + * to takeoff is reached. + * + * @unit deg + * @min 0.0 + * @max 20.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); + +/** + * Max pitch during takeoff. + * Fixed-wing settings are used if set to 0. Note that there is also a minimum + * pitch of 10 degrees during takeoff, so this must be larger if set. + * + * @unit deg + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); + +/** + * Max roll during climbout. + * Roll is limited during climbout to ensure enough lift and prevents aggressive + * navigation before we're on a safe height. + * + * @unit deg + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); + +/** + * Min. airspeed scaling factor for takeoff. + * Pitch up will be commanded when the following airspeed is reached: + * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL + * + * @min 0.0 + * @max 2.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3); diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp new file mode 100644 index 000000000000..64a747f53209 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -0,0 +1,201 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.cpp + * A terrain estimation kalman filter. + */ + +#include "terrain_estimator.h" + +#define DISTANCE_TIMEOUT 100000 // time in usec after which laser is considered dead + +TerrainEstimator::TerrainEstimator() : + _distance_last(0.0f), + _terrain_valid(false), + _time_last_distance(0), + _time_last_gps(0) +{ + memset(&_x._data[0], 0, sizeof(_x._data)); + _u_z = 0.0f; + _P.setIdentity(); +} + +bool TerrainEstimator::is_distance_valid(float distance) +{ + if (distance > 40.0f || distance < 0.00001f) { + return false; + + } else { + return true; + } +} + +void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, + const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance) +{ + if (attitude->R_valid) { + matrix::Matrix R_att(attitude->R); + matrix::Vector a(&sensor->accelerometer_m_s2[0]); + matrix::Vector u; + u = R_att * a; + _u_z = u(2) + 9.81f; // compensate for gravity + + } else { + _u_z = 0.0f; + } + + // dynamics matrix + matrix::Matrix A; + A.setZero(); + A(0, 1) = 1; + A(1, 2) = 1; + + // input matrix + matrix::Matrix B; + B.setZero(); + B(1, 0) = 1; + + // input noise variance + float R = 0.135f; + + // process noise convariance + matrix::Matrix Q; + Q(0, 0) = 0; + Q(1, 1) = 0; + + // do prediction + matrix::Vector dx = (A * _x) * dt; + dx(1) += B(1, 0) * _u_z * dt; + + // propagate state and covariance matrix + _x += dx; + _P += (A * _P + _P * A.transpose() + + B * R * B.transpose() + Q) * dt; +} + +void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude) +{ + // terrain estimate is invalid if we have range sensor timeout + if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) { + _terrain_valid = false; + } + + if (distance->timestamp > _time_last_distance) { + + float d = distance->current_distance; + + matrix::Matrix C; + C(0, 0) = -1; // measured altitude, + + float R = 0.009f; + + matrix::Vector y; + y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch); + + // residual + matrix::Matrix S_I = (C * _P * C.transpose()); + S_I(0, 0) += R; + S_I = matrix::inv (S_I); + matrix::Vector r = y - C * _x; + + matrix::Matrix K = _P * C.transpose() * S_I; + + // some sort of outlayer rejection + if (fabsf(distance->current_distance - _distance_last) < 1.0f) { + _x += K * r; + _P -= K * C * _P; + } + + // if the current and the last range measurement are bad then we consider the terrain + // estimate to be invalid + if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) { + _terrain_valid = false; + + } else { + _terrain_valid = true; + } + + _time_last_distance = distance->timestamp; + _distance_last = distance->current_distance; + } + + if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { + matrix::Matrix C; + C(0, 1) = 1; + + float R = 0.056f; + + matrix::Vector y; + y(0) = gps->vel_d_m_s; + + // residual + matrix::Matrix S_I = (C * _P * C.transpose()); + S_I(0, 0) += R; + S_I = matrix::inv(S_I); + matrix::Vector r = y - C * _x; + + matrix::Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + + _time_last_gps = gps->timestamp_position; + } + + // reinitialise filter if we find bad data + bool reinit = false; + + for (int i = 0; i < n_x; i++) { + if (!PX4_ISFINITE(_x(i))) { + reinit = true; + } + } + + for (int i = 0; i < n_x; i++) { + for (int j = 0; j < n_x; j++) { + if (!PX4_ISFINITE(_P(i, j))) { + reinit = true; + } + } + } + + if (reinit) { + memset(&_x._data[0], 0, sizeof(_x._data)); + _P.setZero(); + _P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f; + } + +} diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h new file mode 100644 index 000000000000..ed9e9741f372 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.h + */ + +#include +#include "matrix/Matrix.hpp" +#include +#include +#include +#include + + +/* +* This class can be used to estimate distance to the ground using a laser range finder. +* It's assumed that the laser points down vertically if the vehicle is in it's neutral pose. +* The predict(...) function will do a state prediciton based on accelerometer inputs. It also +* considers accelerometer bias. +* The measurement_update(...) function does a measurement update based on range finder and gps +* velocity measurements. Both functions should always be called together when there is new +* acceleration data available. +* The is_valid() function provides information whether the estimate is valid. +*/ + +class __EXPORT TerrainEstimator +{ +public: + TerrainEstimator(); + ~TerrainEstimator() {}; + + bool is_valid() {return _terrain_valid;} + float get_distance_to_ground() {return -_x(0);} + float get_velocity() {return _x(1);}; + + void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance); + void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude); + +private: + enum {n_x = 3}; + + float _distance_last; + bool _terrain_valid; + + // kalman filter variables + matrix::Vector _x; // state: ground distance, velocity, accel bias in z direction + float _u_z; // acceleration in earth z direction + matrix::Matrix _P; // covariance matrix + + // timestamps + uint64_t _time_last_distance; + uint64_t _time_last_gps; + + /* + struct { + float var_acc_z; + float var_p_z; + float var_p_vz; + float var_lidar; + float var_gps_vz; + } _params; + */ + + bool is_distance_valid(float distance); + +}; \ No newline at end of file diff --git a/src/systemcmds/bl_update/CMakeLists.txt b/src/systemcmds/bl_update/CMakeLists.txt new file mode 100644 index 000000000000..5b9d7953e518 --- /dev/null +++ b/src/systemcmds/bl_update/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__bl_update + MAIN bl_update + STACK 4096 + COMPILE_FLAGS + -Os + SRCS + bl_update.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/config/CMakeLists.txt b/src/systemcmds/config/CMakeLists.txt new file mode 100644 index 000000000000..c2e876ff2841 --- /dev/null +++ b/src/systemcmds/config/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__config + MAIN config + STACK 4096 + COMPILE_FLAGS + -Os + SRCS + config.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/dumpfile/CMakeLists.txt b/src/systemcmds/dumpfile/CMakeLists.txt new file mode 100644 index 000000000000..5935d7ec06ac --- /dev/null +++ b/src/systemcmds/dumpfile/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__dumpfile + MAIN dumpfile + COMPILE_FLAGS + -Os + SRCS + dumpfile.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/esc_calib/CMakeLists.txt b/src/systemcmds/esc_calib/CMakeLists.txt new file mode 100644 index 000000000000..229271b1d870 --- /dev/null +++ b/src/systemcmds/esc_calib/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__esc_calib + MAIN esc_calib + STACK 4096 + COMPILE_FLAGS + -Os + SRCS + esc_calib.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/i2c/CMakeLists.txt b/src/systemcmds/i2c/CMakeLists.txt new file mode 100644 index 000000000000..04a2b00884c6 --- /dev/null +++ b/src/systemcmds/i2c/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__i2c + MAIN i2c + COMPILE_FLAGS + -Os + SRCS + i2c.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/mixer/CMakeLists.txt b/src/systemcmds/mixer/CMakeLists.txt new file mode 100644 index 000000000000..703d8210fb12 --- /dev/null +++ b/src/systemcmds/mixer/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +set(MIXER_CFLAGS -Os) +if(${OS} STREQUAL "qurt") + list(APPEND MIXER_CFLAGS -Wframe-larger-than=2176) +else() + list(APPEND MIXER_CFLAGS -Wframe-larger-than=2100) +endif() + +px4_add_module( + MODULE systemcmds__mixer + MAIN mixer + STACK 4096 + COMPILE_FLAGS ${MIXER_CFLAGS} + SRCS + mixer.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/motor_test/CMakeLists.txt b/src/systemcmds/motor_test/CMakeLists.txt new file mode 100644 index 000000000000..77e990517824 --- /dev/null +++ b/src/systemcmds/motor_test/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__motor_test + MAIN motor_test + STACK 4096 + COMPILE_FLAGS + -Os + SRCS + motor_test.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/mtd/CMakeLists.txt b/src/systemcmds/mtd/CMakeLists.txt new file mode 100644 index 000000000000..7c671967363f --- /dev/null +++ b/src/systemcmds/mtd/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__mtd + MAIN mtd + COMPILE_FLAGS + -Wno-error + -Os + SRCS + mtd.c + 24xxxx_mtd.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/nshterm/CMakeLists.txt b/src/systemcmds/nshterm/CMakeLists.txt new file mode 100644 index 000000000000..30c2e58a519c --- /dev/null +++ b/src/systemcmds/nshterm/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__nshterm + MAIN nshterm + PRIORITY "SCHED_PRIORITY_DEFAULT-30" + STACK 1500 + COMPILE_FLAGS + -Os + SRCS + nshterm.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/param/CMakeLists.txt b/src/systemcmds/param/CMakeLists.txt new file mode 100644 index 000000000000..cd35b0da04b8 --- /dev/null +++ b/src/systemcmds/param/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__param + MAIN param + STACK 1800 + COMPILE_FLAGS + -Os + SRCS + param.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/perf/CMakeLists.txt b/src/systemcmds/perf/CMakeLists.txt new file mode 100644 index 000000000000..8a0a3a815c54 --- /dev/null +++ b/src/systemcmds/perf/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__perf + MAIN perf + STACK 1800 + COMPILE_FLAGS + -Os + SRCS + perf.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/pwm/CMakeLists.txt b/src/systemcmds/pwm/CMakeLists.txt new file mode 100644 index 000000000000..df1c6088c97c --- /dev/null +++ b/src/systemcmds/pwm/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__pwm + MAIN pwm + STACK 1800 + COMPILE_FLAGS + -Os + SRCS + pwm.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/reboot/CMakeLists.txt b/src/systemcmds/reboot/CMakeLists.txt new file mode 100644 index 000000000000..635b88b95d66 --- /dev/null +++ b/src/systemcmds/reboot/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__reboot + MAIN reboot + STACK 800 + COMPILE_FLAGS + -Os + SRCS + reboot.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/reflect/CMakeLists.txt b/src/systemcmds/reflect/CMakeLists.txt new file mode 100644 index 000000000000..f7fcda35cd12 --- /dev/null +++ b/src/systemcmds/reflect/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__reflect + MAIN reflect + COMPILE_FLAGS + -Os + SRCS + reflect.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt new file mode 100644 index 000000000000..25cad8505c97 --- /dev/null +++ b/src/systemcmds/tests/CMakeLists.txt @@ -0,0 +1,81 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +set(srcs + test_adc.c + test_bson.c + test_float.c + test_gpio.c + test_hott_telemetry.c + test_hrt.c + test_int.c + test_jig_voltages.c + test_led.c + test_sensors.c + test_servo.c + test_sleep.c + test_uart_baudchange.c + test_uart_console.c + test_uart_loopback.c + test_uart_send.c + test_mixer.cpp + test_mathlib.cpp + test_file.c + test_file2.c + tests_main.c + test_params.c + test_ppm_loopback.c + test_rc.c + test_conv.cpp + test_mount.c + test_eigen.cpp + ) + +if(${OS} STREQUAL "nuttx") + list(APPEND srcs + test_time.c + ) +endif() + +px4_add_module( + MODULE systemcmds__tests + MAIN tests + STACK 60000 + COMPILE_FLAGS + -Wframe-larger-than=6000 + -O0 + SRCS ${srcs} + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/tests/test_params.c b/src/systemcmds/tests/test_params.c new file mode 100644 index 000000000000..09845ee25c50 --- /dev/null +++ b/src/systemcmds/tests/test_params.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 test_param.c + * + * Tests related to the parameter system. + */ + +#include +#include +#include "systemlib/err.h" +#include "systemlib/param/param.h" +#include "tests.h" + +#define PARAM_MAGIC1 0x12345678 +#define PARAM_MAGIC2 0xa5a5a5a5 +PARAM_DEFINE_INT32(test, PARAM_MAGIC1); + +int +test_param(int argc, char *argv[]) +{ + param_t p; + + p = param_find("test"); + + if (p == PARAM_INVALID) { + warnx("test parameter not found"); + return 1; + } + + if (param_reset(p) != OK) { + warnx("failed param reset"); + return 1; + } + + param_type_t t = param_type(p); + + if (t != PARAM_TYPE_INT32) { + warnx("test parameter type mismatch (got %u)", (unsigned)t); + return 1; + } + + int32_t val; + + if (param_get(p, &val) != OK) { + warnx("failed to read test parameter"); + return 1; + } + + if (val != PARAM_MAGIC1) { + warnx("parameter value mismatch"); + return 1; + } + + val = PARAM_MAGIC2; + + if (param_set(p, &val) != OK) { + warnx("failed to write test parameter"); + return 1; + } + + if (param_get(p, &val) != OK) { + warnx("failed to re-read test parameter"); + return 1; + } + + if ((uint32_t)val != PARAM_MAGIC2) { + warnx("parameter value mismatch after write"); + return 1; + } + + warnx("parameter test PASS"); + + return 0; +} diff --git a/src/systemcmds/top/CMakeLists.txt b/src/systemcmds/top/CMakeLists.txt new file mode 100644 index 000000000000..0bf73a923cd1 --- /dev/null +++ b/src/systemcmds/top/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__top + MAIN top + STACK 1700 + COMPILE_FLAGS + -Os + SRCS + top.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/topic_listener/CMakeLists.txt b/src/systemcmds/topic_listener/CMakeLists.txt new file mode 100644 index 000000000000..eb3071f41b56 --- /dev/null +++ b/src/systemcmds/topic_listener/CMakeLists.txt @@ -0,0 +1,55 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_custom_command(OUTPUT topic_listener.cpp + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/generate_listener.py ${CMAKE_SOURCE_DIR} > topic_listener.cpp + ) + +add_custom_target(generate_topic_listener + DEPENDS + topic_listener.cpp + ${CMAKE_SOURCE_DIR}/Tools/generate_listener.py) + +px4_add_module( + MODULE systemcmds__topic_listener + MAIN listener + STACK 1800 + COMPILE_FLAGS + -Os + SRCS + topic_listener.cpp + DEPENDS + platforms__common + generate_topic_listener + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/usb_connected/CMakeLists.txt b/src/systemcmds/usb_connected/CMakeLists.txt new file mode 100644 index 000000000000..7d9966229e82 --- /dev/null +++ b/src/systemcmds/usb_connected/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__usb_connected + MAIN usb_connected + COMPILE_FLAGS + -Os + SRCS + usb_connected.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/ver/CMakeLists.txt b/src/systemcmds/ver/CMakeLists.txt new file mode 100644 index 000000000000..f6676b18838b --- /dev/null +++ b/src/systemcmds/ver/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__ver + MAIN ver + STACK 1024 + COMPILE_FLAGS + -Os + SRCS + ver.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/template_.cproject b/template_.cproject new file mode 100644 index 000000000000..a0577f332925 --- /dev/null +++ b/template_.cproject @@ -0,0 +1,178 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + make + archives + true + true + true + + + make + + px4fmu-v2_default + true + true + true + + + make + upload px4fmu-v2_default + true + true + true + + + make + + all + true + true + true + + + make + + posix + true + true + true + + + make + + px4fmu-v1_default + true + true + true + + + make + + upload px4fmu-v1_default + true + true + true + + + make + + clean + true + true + true + + + make + + distclean + true + true + true + + + + diff --git a/template_.project b/template_.project new file mode 100644 index 000000000000..7a81d46ab913 --- /dev/null +++ b/template_.project @@ -0,0 +1,38 @@ + + + PX4-Firmware + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + + + + 1438876028862 + + 26 + + org.eclipse.ui.ide.multiFilter + 1.0-name-matches-false-false-.git + + + + From e83d15f01968902d55cda072b23be983a0ffe427 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 08:56:19 +1100 Subject: [PATCH 267/293] merge: added missing files from upstream --- Tools/parameters_injected.xml | 138 ++++++++++++++++++++++ Tools/posix.gdbinit | 2 + Tools/posix_lldbinit | 3 + src/drivers/boards/sitl/CMakeLists.txt | 42 +++++++ src/drivers/camera_trigger/CMakeLists.txt | 44 +++++++ src/drivers/pwm_out_sim/CMakeLists.txt | 44 +++++++ src/drivers/srf02/CMakeLists.txt | 43 +++++++ src/firmware/nuttx/CMakeLists.txt | 112 ++++++++++++++++++ src/firmware/posix/CMakeLists.txt | 63 ++++++++++ src/firmware/qurt/CMakeLists.txt | 31 +++++ src/lib/runway_takeoff/CMakeLists.txt | 43 +++++++ src/lib/terrain_estimation/CMakeLists.txt | 42 +++++++ 12 files changed, 607 insertions(+) create mode 100644 Tools/parameters_injected.xml create mode 100644 Tools/posix.gdbinit create mode 100644 Tools/posix_lldbinit create mode 100644 src/drivers/boards/sitl/CMakeLists.txt create mode 100644 src/drivers/camera_trigger/CMakeLists.txt create mode 100644 src/drivers/pwm_out_sim/CMakeLists.txt create mode 100644 src/drivers/srf02/CMakeLists.txt create mode 100644 src/firmware/nuttx/CMakeLists.txt create mode 100644 src/firmware/posix/CMakeLists.txt create mode 100644 src/firmware/qurt/CMakeLists.txt create mode 100644 src/lib/runway_takeoff/CMakeLists.txt create mode 100644 src/lib/terrain_estimation/CMakeLists.txt diff --git a/Tools/parameters_injected.xml b/Tools/parameters_injected.xml new file mode 100644 index 000000000000..2f164ee52b72 --- /dev/null +++ b/Tools/parameters_injected.xml @@ -0,0 +1,138 @@ + + + 3 + + + Speed controller bandwidth + Speed controller bandwidth, in Hz. Higher values result in faster speed and current rise times, but may result in overshoot and higher current consumption. For fixed-wing aircraft, this value should be less than 50 Hz; for multirotors, values up to 100 Hz may provide improvements in responsiveness. + Hertz + 10 + 250 + + + Reverse direction + Motor spin direction as detected during initial enumeration. Use 0 or 1 to reverse direction. + 0 + 1 + + + Speed (RPM) controller gain + Speed (RPM) controller gain. Determines controller + aggressiveness; units are amp-seconds per radian. Systems with + higher rotational inertia (large props) will need gain increased; + systems with low rotational inertia (small props) may need gain + decreased. Higher values result in faster response, but may result + in oscillation and excessive overshoot. Lower values result in a + slower, smoother response. + amp-seconds per radian + 3 + 0.00 + 1.00 + + + Idle speed (e Hz) + Idle speed (e Hz) + Hertz + 3 + 0.0 + 100.0 + + + Spin-up rate (e Hz/s) + Spin-up rate (e Hz/s) + Hz/s + 5 + 1000 + + + Index of this ESC in throttle command messages. + Index of this ESC in throttle command messages. + Index + 0 + 15 + + + Extended status ID + Extended status ID + 1 + 1000000 + + + Extended status interval (µs) + Extended status interval (µs) + µs + 0 + 1000000 + + + ESC status interval (µs) + ESC status interval (µs) + µs + 1000000 + + + Motor current limit in amps + Motor current limit in amps. This determines the maximum + current controller setpoint, as well as the maximum allowable + current setpoint slew rate. This value should generally be set to + the continuous current rating listed in the motor’s specification + sheet, or set equal to the motor’s specified continuous power + divided by the motor voltage limit. + Amps + 3 + 1 + 80 + + + Motor Kv in RPM per volt + Motor Kv in RPM per volt. This can be taken from the motor’s + specification sheet; accuracy will help control performance but + some deviation from the specified value is acceptable. + RPM/v + 0 + 4000 + + + READ ONLY: Motor inductance in henries. + READ ONLY: Motor inductance in henries. This is measured on start-up. + henries + 3 + + + Number of motor poles. + Number of motor poles. Used to convert mechanical speeds to + electrical speeds. This number should be taken from the motor’s + specification sheet. + Poles + 2 + 40 + + + READ ONLY: Motor resistance in ohms + READ ONLY: Motor resistance in ohms. This is measured on start-up. When + tuning a new motor, check that this value is approximately equal + to the value shown in the motor’s specification sheet. + Ohms + 3 + + + Acceleration limit (V) + Acceleration limit (V) + Volts + 3 + 0.01 + 1.00 + + + Motor voltage limit in volts + Motor voltage limit in volts. The current controller’s + commanded voltage will never exceed this value. Note that this may + safely be above the nominal voltage of the motor; to determine the + actual motor voltage limit, divide the motor’s rated power by the + motor current limit. + Volts + 3 + 0 + + + \ No newline at end of file diff --git a/Tools/posix.gdbinit b/Tools/posix.gdbinit new file mode 100644 index 000000000000..23fa44b4e0c6 --- /dev/null +++ b/Tools/posix.gdbinit @@ -0,0 +1,2 @@ +handle SIGCONT nostop noprint nopass +run diff --git a/Tools/posix_lldbinit b/Tools/posix_lldbinit new file mode 100644 index 000000000000..a3e04d3a07da --- /dev/null +++ b/Tools/posix_lldbinit @@ -0,0 +1,3 @@ +run +pro hand -p true -s false -n false SIGCONT +continue diff --git a/src/drivers/boards/sitl/CMakeLists.txt b/src/drivers/boards/sitl/CMakeLists.txt new file mode 100644 index 000000000000..8d7d7bee3423 --- /dev/null +++ b/src/drivers/boards/sitl/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__sitl + COMPILE_FLAGS + -Os + SRCS + sitl_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/camera_trigger/CMakeLists.txt b/src/drivers/camera_trigger/CMakeLists.txt new file mode 100644 index 000000000000..4f3669876e4d --- /dev/null +++ b/src/drivers/camera_trigger/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__camera_trigger + MAIN camera_trigger + COMPILE_FLAGS + -Os + SRCS + camera_trigger.cpp + camera_trigger_params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/pwm_out_sim/CMakeLists.txt b/src/drivers/pwm_out_sim/CMakeLists.txt new file mode 100644 index 000000000000..0febaa0beac1 --- /dev/null +++ b/src/drivers/pwm_out_sim/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__pwm_out_sim + MAIN pwm_out_sim + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + pwm_out_sim.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/srf02/CMakeLists.txt b/src/drivers/srf02/CMakeLists.txt new file mode 100644 index 000000000000..da5b8786961f --- /dev/null +++ b/src/drivers/srf02/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__srf02 + MAIN srf02 + COMPILE_FLAGS + -Os + SRCS + srf02.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt new file mode 100644 index 000000000000..649ce1e8feb2 --- /dev/null +++ b/src/firmware/nuttx/CMakeLists.txt @@ -0,0 +1,112 @@ +px4_nuttx_generate_builtin_commands( + OUT builtin_commands.c + MODULE_LIST + ${module_libraries} + ${config_extra_builtin_cmds} + ) + +# add executable +add_executable(firmware_nuttx + builtin_commands.c) + + +set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${BOARD}/NuttX/nuttx-export) + +set(link_libs + romfs apps nuttx m gcc + ) + +if(NOT ${BOARD} STREQUAL "sim") + list(APPEND link_libs nosys) + set(main_link_flags + "-T${nuttx_export_dir}/build/ld.script" + "-Wl,-Map=${CMAKE_BINARY_DIR}/${BOARD}/main.map" + ) + px4_join(OUT main_link_flags LIST ${main_link_flags} GLUE " ") + set_target_properties(firmware_nuttx PROPERTIES LINK_FLAGS ${main_link_flags}) +endif() + +target_link_libraries(firmware_nuttx + -Wl,--start-group + ${module_libraries} + ${config_extra_libs} + ${link_libs} + -Wl,--end-group) + +add_custom_target(check_weak + COMMAND ${NM} firmware_nuttx | ${GREP} " w " | cat + DEPENDS firmware_nuttx + VERBATIM + ) + +if(NOT ${BOARD} STREQUAL "sim") + + if (config_io_board) + set(extras "${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin") + endif() + + px4_nuttx_add_romfs(OUT romfs + ROOT ROMFS/px4fmu_common + EXTRAS ${extras} + ) + if (config_io_board) + add_dependencies(romfs fw_io) + endif() + set(fw_file + ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) + + px4_nuttx_add_firmware(OUT ${fw_file} + BOARD ${BOARD} + EXE ${CMAKE_CURRENT_BINARY_DIR}/firmware_nuttx + PARAM_XML ${CMAKE_BINARY_DIR}/parameters.xml + AIRFRAMES_XML ${CMAKE_BINARY_DIR}/airframes.xml + ) + + configure_file(gdbinit.in .gdbinit) + + add_custom_target(debug + COMMAND ${GDB} ${CMAKE_CURRENT_BINARY_DIR}/firmware_nuttx + DEPENDS firmware_nuttx + ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit + ) + + add_custom_target(debug_tui + COMMAND ${GDBTUI} ${CMAKE_CURRENT_BINARY_DIR}/firmware_nuttx + DEPENDS firmware_nuttx + ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit + ) + + add_custom_target(debug_ddd + COMMAND ${DDD} --debugger ${GDB} ${CMAKE_CURRENT_BINARY_DIR}/firmware_nuttx + DEPENDS firmware_nuttx + ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit + ) + + add_custom_target(debug_io + COMMAND ${GDB} + ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board} + DEPENDS firmware_nuttx + ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit + ) + + add_custom_target(debug_io_tui + COMMAND ${GDBTUI} + ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board} + DEPENDS firmware_nuttx + ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit + ) + + add_custom_target(debug_io_ddd + COMMAND ${DDD} --debugger ${GDB} + ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board} + DEPENDS firmware_nuttx + ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit + ) + + px4_add_upload(OUT upload OS ${OS} BOARD ${BOARD} + BUNDLE ${fw_file}) +endif() + +install(FILES ${fw_file} DESTINATION .) + +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt new file mode 100644 index 000000000000..efc81d633510 --- /dev/null +++ b/src/firmware/posix/CMakeLists.txt @@ -0,0 +1,63 @@ +include_directories(${CMAKE_CURRENT_BINARY_DIR}) + + +px4_posix_generate_builtin_commands( + OUT apps.h + MODULE_LIST ${module_libraries}) + +add_executable(mainapp + ${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp + apps.h + ) + +if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" OR NOT APPLE) + target_link_libraries(mainapp + -Wl,--start-group + ${module_libraries} + pthread m rt + -Wl,--end-group + ) +else() + target_link_libraries(mainapp + ${module_libraries} + pthread m + ) +endif() + +add_custom_target(run_config + COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}" + "${config_sitl_viewer}" "${config_sitl_model}" "${CMAKE_BINARY_DIR}" + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + USES_TERMINAL + ) +add_dependencies(run_config mainapp) + +foreach(viewer none jmavsim gazebo) + foreach(debugger none gdb lldb ddd valgrind) + foreach(model none iris tailsitter) + if (debugger STREQUAL "none") + if (model STREQUAL "none") + set(_targ_name "${viewer}") + else() + set(_targ_name "${viewer}_${model}") + endif() + else() + if (model STREQUAL "none") + set(_targ_name "${viewer}___${debugger}") + else() + set(_targ_name "${viewer}_${model}_${debugger}") + endif() + endif() + add_custom_target(${_targ_name} + COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" + "${debugger}" + "${viewer}" "${model}" "${CMAKE_BINARY_DIR}" + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + USES_TERMINAL + ) + add_dependencies(${_targ_name} mainapp) + endforeach() + endforeach() +endforeach() + +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt new file mode 100644 index 000000000000..b6d743186faf --- /dev/null +++ b/src/firmware/qurt/CMakeLists.txt @@ -0,0 +1,31 @@ +include_directories(${CMAKE_CURRENT_BINARY_DIR}) + +px4_qurt_generate_builtin_commands( + OUT ${CMAKE_BINARY_DIR}/apps.h + MODULE_LIST ${module_libraries}) + +# Clear -rdynamic flag which fails for hexagon +# this change is scoped in this directory and below +set(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS "") +set(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "") + +# Enable build without HexagonSDK to check link dependencies +if ("${QURT_ENABLE_STUBS}" STREQUAL "1") + add_executable(mainapp + ${CMAKE_SOURCE_DIR}/src/platforms/qurt/dspal/dspal_stub.c + ${CMAKE_BINARY_DIR}/apps.h) +else() + add_library(mainapp + ${CMAKE_SOURCE_DIR}/src/platforms/qurt/dspal/dspal_stub.c + ${CMAKE_BINARY_DIR}/apps.h) +endif() + +target_link_libraries(mainapp + -Wl,--whole-archive + ${module_libraries} + ${target_libraries} + m + -Wl,--no-whole-archive + -Wl,${TOOLSLIB}/pic/libstdc++.a) + +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/runway_takeoff/CMakeLists.txt b/src/lib/runway_takeoff/CMakeLists.txt new file mode 100644 index 000000000000..7938279a0e7e --- /dev/null +++ b/src/lib/runway_takeoff/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__runway_takeoff + COMPILE_FLAGS + -Os + SRCS + RunwayTakeoff.cpp + runway_takeoff_params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/terrain_estimation/CMakeLists.txt b/src/lib/terrain_estimation/CMakeLists.txt new file mode 100644 index 000000000000..0c1177b96b27 --- /dev/null +++ b/src/lib/terrain_estimation/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE lib__terrain_estimation + COMPILE_FLAGS + -Os + SRCS + terrain_estimator.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : From 255f3ddeaba49bf685be09748590a47e636c7b5d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 08:58:51 +1100 Subject: [PATCH 268/293] merge: ignore some submodules --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index dddebfe3a452..61d26d7e7eb9 100644 --- a/.gitignore +++ b/.gitignore @@ -53,3 +53,6 @@ src/platforms/qurt/px4_messages/ makefiles/config_px4fmu-v2_APM.mk makefiles/nuttx/config_px4fmu-v1_APM.mk makefiles/nuttx/config_px4fmu-v2_APM.mk +Tools/sitl_gazebo +src/lib/dspal +src/lib/matrix From 9557cc883dacdf5602031993da29582e0a60f4ce Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 12:22:17 +1100 Subject: [PATCH 269/293] px4io-v2: revert to old defconfig the new setup led to a panic when the mixed failed to load and override was enabled. This needs to be investigated, but probably was the change in stack config --- nuttx-configs/px4io-v2/nsh/defconfig | 1128 +++++++++++--------------- 1 file changed, 475 insertions(+), 653 deletions(-) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 56892e3f9319..a9b84779484a 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -1,725 +1,547 @@ +############################################################################ +# configs/px4io/nsh/defconfig +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization # -# Automatically generated file; DO NOT EDIT. -# Nuttx/ Configuration -# -CONFIG_NUTTX_NEWCONFIG=y - -# -# Build Setup -# -# CONFIG_EXPERIMENTAL is not set -CONFIG_HOST_LINUX=y -# CONFIG_HOST_OSX is not set -# CONFIG_HOST_WINDOWS is not set -# CONFIG_HOST_OTHER is not set - -# -# Build Configuration -# -CONFIG_APPS_DIR="../apps" -# CONFIG_BUILD_2PASS is not set - -# -# Binary Output Formats -# -# CONFIG_RRLOAD_BINARY is not set -# CONFIG_INTELHEX_BINARY is not set -# CONFIG_MOTOROLA_SREC is not set -CONFIG_RAW_BINARY=y - -# -# Customize Header Files -# -# CONFIG_ARCH_STDBOOL_H is not set -CONFIG_ARCH_MATH_H=y -# CONFIG_ARCH_FLOAT_H is not set -# CONFIG_ARCH_STDARG_H is not set - -# -# Debug Options -# -# CONFIG_DEBUG is not set -CONFIG_DEBUG_SYMBOLS=y -# -# System Type -# -# CONFIG_ARCH_8051 is not set -CONFIG_ARCH_ARM=y -# CONFIG_ARCH_AVR is not set -# CONFIG_ARCH_HC is not set -# CONFIG_ARCH_MIPS is not set -# CONFIG_ARCH_RGMP is not set -# CONFIG_ARCH_SH is not set -# CONFIG_ARCH_SIM is not set -# CONFIG_ARCH_X86 is not set -# CONFIG_ARCH_Z16 is not set -# CONFIG_ARCH_Z80 is not set CONFIG_ARCH="arm" - -# -# ARM Options -# -# CONFIG_ARCH_CHIP_C5471 is not set -# CONFIG_ARCH_CHIP_CALYPSO is not set -# CONFIG_ARCH_CHIP_DM320 is not set -# CONFIG_ARCH_CHIP_IMX is not set -# CONFIG_ARCH_CHIP_KINETIS is not set -# CONFIG_ARCH_CHIP_KL is not set -# CONFIG_ARCH_CHIP_LM is not set -# CONFIG_ARCH_CHIP_LPC17XX is not set -# CONFIG_ARCH_CHIP_LPC214X is not set -# CONFIG_ARCH_CHIP_LPC2378 is not set -# CONFIG_ARCH_CHIP_LPC31XX is not set -# CONFIG_ARCH_CHIP_LPC43XX is not set -# CONFIG_ARCH_CHIP_NUC1XX is not set -# CONFIG_ARCH_CHIP_SAM34 is not set -CONFIG_ARCH_CHIP_STM32=y -# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_ARM=y CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_FAMILY="armv7-m" CONFIG_ARCH_CHIP="stm32" -# CONFIG_ARMV7M_USEBASEPRI is not set -CONFIG_ARCH_HAVE_CMNVECTOR=y -CONFIG_ARMV7M_CMNVECTOR=y -# CONFIG_ARCH_HAVE_FPU is not set -CONFIG_ARCH_HAVE_MPU=y -# CONFIG_ARMV7M_MPU is not set +CONFIG_ARCH_CHIP_STM32F100C8=y +CONFIG_ARCH_BOARD="px4io-v2" +CONFIG_ARCH_BOARD_PX4IO_V2=y +CONFIG_BOARD_LOOPSPERMSEC=2000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARMV7M_STACKCHECK=n # -# ARMV7M Configuration Options +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): # -# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set -# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set -# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set -CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -# CONFIG_ARMV7M_STACKCHECK is not set -CONFIG_SERIAL_TERMIOS=y - +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG # -# STM32 Configuration Options -# -# CONFIG_ARCH_CHIP_STM32L151C6 is not set -# CONFIG_ARCH_CHIP_STM32L151C8 is not set -# CONFIG_ARCH_CHIP_STM32L151CB is not set -# CONFIG_ARCH_CHIP_STM32L151R6 is not set -# CONFIG_ARCH_CHIP_STM32L151R8 is not set -# CONFIG_ARCH_CHIP_STM32L151RB is not set -# CONFIG_ARCH_CHIP_STM32L151V6 is not set -# CONFIG_ARCH_CHIP_STM32L151V8 is not set -# CONFIG_ARCH_CHIP_STM32L151VB is not set -# CONFIG_ARCH_CHIP_STM32L152C6 is not set -# CONFIG_ARCH_CHIP_STM32L152C8 is not set -# CONFIG_ARCH_CHIP_STM32L152CB is not set -# CONFIG_ARCH_CHIP_STM32L152R6 is not set -# CONFIG_ARCH_CHIP_STM32L152R8 is not set -# CONFIG_ARCH_CHIP_STM32L152RB is not set -# CONFIG_ARCH_CHIP_STM32L152V6 is not set -# CONFIG_ARCH_CHIP_STM32L152V8 is not set -# CONFIG_ARCH_CHIP_STM32L152VB is not set -CONFIG_ARCH_CHIP_STM32F100C8=y -# CONFIG_ARCH_CHIP_STM32F100CB is not set -# CONFIG_ARCH_CHIP_STM32F100R8 is not set -# CONFIG_ARCH_CHIP_STM32F100RB is not set -# CONFIG_ARCH_CHIP_STM32F100RC is not set -# CONFIG_ARCH_CHIP_STM32F100RD is not set -# CONFIG_ARCH_CHIP_STM32F100RE is not set -# CONFIG_ARCH_CHIP_STM32F100V8 is not set -# CONFIG_ARCH_CHIP_STM32F100VB is not set -# CONFIG_ARCH_CHIP_STM32F100VC is not set -# CONFIG_ARCH_CHIP_STM32F100VD is not set -# CONFIG_ARCH_CHIP_STM32F100VE is not set -# CONFIG_ARCH_CHIP_STM32F102CB is not set -# CONFIG_ARCH_CHIP_STM32F103C4 is not set -# CONFIG_ARCH_CHIP_STM32F103C8 is not set -# CONFIG_ARCH_CHIP_STM32F103RET6 is not set -# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set -# CONFIG_ARCH_CHIP_STM32F103VET6 is not set -# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set -# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set -# CONFIG_ARCH_CHIP_STM32F107VC is not set -# CONFIG_ARCH_CHIP_STM32F207IG is not set -# CONFIG_ARCH_CHIP_STM32F302CB is not set -# CONFIG_ARCH_CHIP_STM32F302CC is not set -# CONFIG_ARCH_CHIP_STM32F302RB is not set -# CONFIG_ARCH_CHIP_STM32F302RC is not set -# CONFIG_ARCH_CHIP_STM32F302VB is not set -# CONFIG_ARCH_CHIP_STM32F302VC is not set -# CONFIG_ARCH_CHIP_STM32F303CB is not set -# CONFIG_ARCH_CHIP_STM32F303CC is not set -# CONFIG_ARCH_CHIP_STM32F303RB is not set -# CONFIG_ARCH_CHIP_STM32F303RC is not set -# CONFIG_ARCH_CHIP_STM32F303VB is not set -# CONFIG_ARCH_CHIP_STM32F303VC is not set -# CONFIG_ARCH_CHIP_STM32F405RG is not set -# CONFIG_ARCH_CHIP_STM32F405VG is not set -# CONFIG_ARCH_CHIP_STM32F405ZG is not set -# CONFIG_ARCH_CHIP_STM32F407VE is not set -# CONFIG_ARCH_CHIP_STM32F407VG is not set -# CONFIG_ARCH_CHIP_STM32F407ZE is not set -# CONFIG_ARCH_CHIP_STM32F407ZG is not set -# CONFIG_ARCH_CHIP_STM32F407IE is not set -# CONFIG_ARCH_CHIP_STM32F407IG is not set -# CONFIG_ARCH_CHIP_STM32F427V is not set -# CONFIG_ARCH_CHIP_STM32F427Z is not set -# CONFIG_ARCH_CHIP_STM32F427I is not set -# CONFIG_STM32_STM32L15XX is not set -# CONFIG_STM32_ENERGYLITE is not set -CONFIG_STM32_STM32F10XX=y -CONFIG_STM32_VALUELINE=y -# CONFIG_STM32_CONNECTIVITYLINE is not set -# CONFIG_STM32_PERFORMANCELINE is not set -# CONFIG_STM32_HIGHDENSITY is not set -# CONFIG_STM32_MEDIUMDENSITY is not set -# CONFIG_STM32_LOWDENSITY is not set -# CONFIG_STM32_STM32F20XX is not set -# CONFIG_STM32_STM32F30XX is not set -# CONFIG_STM32_STM32F40XX is not set +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=n +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=y # -# STM32 Peripheral Support +# Individual subsystems can be enabled: # -CONFIG_STM32_ADC1=y -# CONFIG_STM32_BKP is not set -# CONFIG_STM32_CEC is not set -# CONFIG_STM32_CRC is not set +# AHB: CONFIG_STM32_DMA1=y -# CONFIG_STM32_DAC1 is not set -# CONFIG_STM32_DAC2 is not set -# CONFIG_STM32_I2C1 is not set -# CONFIG_STM32_I2C2 is not set -# CONFIG_STM32_PWR is not set -# CONFIG_STM32_SPI1 is not set -# CONFIG_STM32_TIM1 is not set -# CONFIG_STM32_TIM2 is not set -# CONFIG_STM32_TIM3 is not set -# CONFIG_STM32_TIM4 is not set -# CONFIG_STM32_TIM5 is not set -# CONFIG_STM32_TIM6 is not set -# CONFIG_STM32_TIM7 is not set -# CONFIG_STM32_TIM12 is not set -# CONFIG_STM32_TIM13 is not set -# CONFIG_STM32_TIM14 is not set -# CONFIG_STM32_TIM15 is not set -# CONFIG_STM32_TIM16 is not set -# CONFIG_STM32_TIM17 is not set -CONFIG_STM32_USART1=y +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y -# CONFIG_STM32_UART4 is not set -# CONFIG_STM32_UART5 is not set -# CONFIG_STM32_IWDG is not set -# CONFIG_STM32_WWDG is not set -CONFIG_STM32_ADC=y +CONFIG_STM32_I2C1=n +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +# We use our own ADC driver, but leave this on for clocking purposes. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n -# -# Alternate Pin Mapping -# -# CONFIG_STM32_USART1_REMAP is not set -# CONFIG_STM32_USART2_REMAP is not set -CONFIG_STM32_USART3_NO_REMAP=y -# CONFIG_STM32_USART3_FULL_REMAP is not set -# CONFIG_STM32_USART3_PARTIAL_REMAP is not set -# CONFIG_STM32_JTAG_DISABLE is not set -# CONFIG_STM32_JTAG_FULL_ENABLE is not set -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set -CONFIG_STM32_JTAG_SW_ENABLE=y -# CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG is not set -# CONFIG_STM32_FORCEPOWER is not set -# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -# CONFIG_STM32_DMACAPABLE is not set -CONFIG_STM32_USART=y # -# U[S]ART Configuration +# STM32F100 specific serial device driver settings # -# CONFIG_USART1_RS485 is not set -# CONFIG_USART2_RS485 is not set -# CONFIG_USART2_RXDMA is not set -# CONFIG_USART3_RS485 is not set -CONFIG_USART3_RXDMA=y -# CONFIG_SERIAL_DISABLE_REORDERING is not set -# CONFIG_STM32_USART_SINGLEWIRE is not set - -# -# USB Host Configuration +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits # +CONFIG_SERIAL_TERMIOS=y +CONFIG_STANDARD_SERIAL=y -# -# USB Device Configuration -# +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n -# -# External Memory Configuration -# +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 -# -# Architecture Options -# -# CONFIG_ARCH_NOINTC is not set -# CONFIG_ARCH_VECNOTIRQ is not set -CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y -# CONFIG_CUSTOM_STACK is not set -# CONFIG_ADDRENV is not set -CONFIG_ARCH_HAVE_VFORK=y -CONFIG_ARCH_STACKDUMP=y -# CONFIG_ENDIAN_BIG is not set -# CONFIG_ARCH_HAVE_RAMFUNCS is not set -CONFIG_ARCH_HAVE_RAMVECTORS=y -# CONFIG_ARCH_RAMVECTORS is not set +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 -# -# Board Settings -# -CONFIG_BOARD_LOOPSPERMSEC=2000 -# CONFIG_ARCH_CALIBRATION is not set -CONFIG_DRAM_START=0x20000000 -CONFIG_DRAM_SIZE=0x2000 -CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=500 +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 -# -# Boot options -# -# CONFIG_BOOT_RUNFROMEXTSRAM is not set -CONFIG_BOOT_RUNFROMFLASH=y -# CONFIG_BOOT_RUNFROMISRAM is not set -# CONFIG_BOOT_RUNFROMSDRAM is not set -# CONFIG_BOOT_COPYTORAM is not set +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 -# -# Board Selection -# -CONFIG_ARCH_BOARD="px4io-v2" -CONFIG_ARCH_BOARD_PX4IO_V2=y +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 + +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +# Conflicts with I2C1 DMA +CONFIG_USART2_RXDMA=n +CONFIG_USART3_RXDMA=y # -# Common Board Options +# PX4IO specific driver settings # - +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. # -# Board-Specific Options # +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y # -# RTOS Features +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API # -# CONFIG_BOARD_INITIALIZE is not set +CONFIG_USER_ENTRYPOINT="user_start" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + CONFIG_MSEC_PER_TICK=1 +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y CONFIG_RR_INTERVAL=0 -# CONFIG_SCHED_INSTRUMENTATION is not set +CONFIG_SCHED_INSTRUMENTATION=n CONFIG_TASK_NAME_SIZE=8 -# CONFIG_SCHED_HAVE_PARENT is not set -# CONFIG_JULIAN_TIME is not set CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +# this eats ~1KiB of RAM ... work out why CONFIG_DEV_CONSOLE=y -# CONFIG_MUTEX_TYPES is not set -# CONFIG_PRIORITY_INHERITANCE is not set +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 CONFIG_FDCLONE_DISABLE=y CONFIG_FDCLONE_STDIO=y CONFIG_SDCLONE_DISABLE=y -# CONFIG_SCHED_WAITPID is not set -# CONFIG_SCHED_STARTHOOK is not set -# CONFIG_SCHED_ATEXIT is not set -# CONFIG_SCHED_ONEXIT is not set -CONFIG_USER_ENTRYPOINT="user_start" -CONFIG_DISABLE_OS_API=y -# CONFIG_DISABLE_CLOCK is not set +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=50000 +CONFIG_SCHED_WORKSTACKSIZE=1024 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=n +CONFIG_SCHED_ATEXIT=n + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y CONFIG_DISABLE_SIGNALS=y CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y - -# -# Sizes of configurable things (0 disables) -# -CONFIG_MAX_TASKS=4 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=2 -CONFIG_NFILE_DESCRIPTORS=8 -CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=12 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=4 -CONFIG_PREALLOC_TIMERS=0 - -# -# Stack and heap information -# -CONFIG_IDLETHREAD_STACKSIZE=290 -CONFIG_USERMAIN_STACKSIZE=800 -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_PTHREAD_STACK_DEFAULT=1024 - -# -# Device Drivers -# CONFIG_DISABLE_POLL=y -CONFIG_DEV_NULL=y -# CONFIG_DEV_ZERO is not set -# CONFIG_LOOP is not set -# CONFIG_RAMDISK is not set -# CONFIG_CAN is not set -# CONFIG_PWM is not set -# CONFIG_I2C is not set -CONFIG_ARCH_HAVE_I2CRESET=y -# CONFIG_SPI is not set -# CONFIG_RTC is not set -# CONFIG_WATCHDOG is not set -# CONFIG_ANALOG is not set -# CONFIG_AUDIO_DEVICES is not set -# CONFIG_BCH is not set -# CONFIG_INPUT is not set -# CONFIG_LCD is not set -# CONFIG_MMCSD is not set -# CONFIG_MTD is not set -# CONFIG_PIPES is not set -# CONFIG_PM is not set -# CONFIG_POWER is not set -# CONFIG_SENSORS is not set -CONFIG_SERIAL=y -# CONFIG_DEV_LOWCONSOLE is not set -# CONFIG_16550_UART is not set -CONFIG_ARCH_HAVE_USART1=y -CONFIG_ARCH_HAVE_USART2=y -CONFIG_ARCH_HAVE_USART3=y -CONFIG_MCU_SERIAL=y -CONFIG_STANDARD_SERIAL=y -CONFIG_USART1_SERIAL_CONSOLE=y -# CONFIG_USART2_SERIAL_CONSOLE is not set -# CONFIG_USART3_SERIAL_CONSOLE is not set -# CONFIG_NO_SERIAL_CONSOLE is not set - -# -# USART1 Configuration -# -CONFIG_USART1_RXBUFSIZE=64 -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART1_BAUD=115200 -CONFIG_USART1_BITS=8 -CONFIG_USART1_PARITY=0 -CONFIG_USART1_2STOP=0 -# CONFIG_USART1_IFLOWCONTROL is not set -# CONFIG_USART1_OFLOWCONTROL is not set - -# -# USART2 Configuration -# -CONFIG_USART2_RXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART2_BAUD=115200 -CONFIG_USART2_BITS=8 -CONFIG_USART2_PARITY=0 -CONFIG_USART2_2STOP=0 -# CONFIG_USART2_IFLOWCONTROL is not set -# CONFIG_USART2_OFLOWCONTROL is not set - -# -# USART3 Configuration -# -CONFIG_USART3_RXBUFSIZE=64 -CONFIG_USART3_TXBUFSIZE=64 -CONFIG_USART3_BAUD=115200 -CONFIG_USART3_BITS=8 -CONFIG_USART3_PARITY=0 -CONFIG_USART3_2STOP=0 -# CONFIG_USART3_IFLOWCONTROL is not set -# CONFIG_USART3_OFLOWCONTROL is not set -# CONFIG_SERIAL_IFLOWCONTROL is not set -# CONFIG_SERIAL_OFLOWCONTROL is not set -# CONFIG_USBDEV is not set -# CONFIG_USBHOST is not set -# CONFIG_WIRELESS is not set - -# -# System Logging Device Options -# - -# -# System Logging -# -# CONFIG_RAMLOG is not set - -# -# Networking Support -# -# CONFIG_NET is not set - -# -# File Systems -# - -# -# File system configuration -# -CONFIG_DISABLE_MOUNTPOINT=y -# CONFIG_FS_RAMMAP is not set # -# System Logging -# -# CONFIG_SYSLOG_ENABLE is not set -# CONFIG_SYSLOG is not set - -# -# Graphics Support -# -# CONFIG_NX is not set - +# Misc libc settings # -# Memory Management +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes # -# CONFIG_MM_MULTIHEAP is not set -CONFIG_MM_SMALL=y -CONFIG_MM_REGIONS=1 -# CONFIG_GRAN is not set +CONFIG_NOPRINTF_FIELDWIDTH=n # -# Audio Support -# -# CONFIG_AUDIO is not set - +# Allow for architecture optimized implementations # -# Binary Formats +# The architecture can provide optimized versions of the +# following to improve system performance # -# CONFIG_BINFMT_DISABLE is not set -# CONFIG_NXFLAT is not set -# CONFIG_ELF is not set -CONFIG_BUILTIN=y -# CONFIG_PIC is not set -# CONFIG_SYMTAB_ORDEREDBYNAME is not set +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n # -# Library Routines -# - +# Sizes of configurable things (0 disables) # -# Standard C Library Options +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. # +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 CONFIG_STDIO_BUFFER_SIZE=32 -# CONFIG_STDIO_LINEBUFFER is not set +CONFIG_STDIO_LINEBUFFER=n CONFIG_NUNGET_CHARS=2 -# CONFIG_NOPRINTF_FIELDWIDTH is not set -# CONFIG_LIBC_FLOATINGPOINT is not set -CONFIG_LIB_RAND_ORDER=1 -# CONFIG_EOL_IS_CR is not set -# CONFIG_EOL_IS_LF is not set -# CONFIG_EOL_IS_BOTH_CRLF is not set -CONFIG_EOL_IS_EITHER_CRLF=y -# CONFIG_LIBC_EXECFUNCS is not set -CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 -CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 -# CONFIG_LIBC_STRERROR is not set -# CONFIG_LIBC_PERROR_STDOUT is not set -CONFIG_ARCH_LOWPUTC=y -CONFIG_LIB_SENDFILE_BUFSIZE=512 -# CONFIG_ARCH_ROMGETC is not set -# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set - -# -# Non-standard Library Support -# -# CONFIG_LIB_KBDCODEC is not set -# CONFIG_LIB_SLCDCODEC is not set - -# -# Basic CXX Support -# -# CONFIG_C99_BOOL8 is not set -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -# CONFIG_CXX_NEWLONG is not set - -# -# uClibc++ Standard C++ Library -# -# CONFIG_UCLIBCXX is not set - -# -# Application Configuration -# - -# -# Built-In Applications -# - -# -# Examples -# -# CONFIG_EXAMPLES_BUTTONS is not set -# CONFIG_EXAMPLES_CAN is not set -# CONFIG_EXAMPLES_COMPOSITE is not set -# CONFIG_EXAMPLES_CXXTEST is not set -# CONFIG_EXAMPLES_DHCPD is not set -# CONFIG_EXAMPLES_ELF is not set -# CONFIG_EXAMPLES_FTPC is not set -# CONFIG_EXAMPLES_FTPD is not set -# CONFIG_EXAMPLES_HELLO is not set -# CONFIG_EXAMPLES_HELLOXX is not set -# CONFIG_EXAMPLES_JSON is not set -# CONFIG_EXAMPLES_HIDKBD is not set -# CONFIG_EXAMPLES_KEYPADTEST is not set -# CONFIG_EXAMPLES_IGMP is not set -# CONFIG_EXAMPLES_LCDRW is not set -# CONFIG_EXAMPLES_MM is not set -# CONFIG_EXAMPLES_MODBUS is not set -# CONFIG_EXAMPLES_MOUNT is not set -# CONFIG_EXAMPLES_NRF24L01TERM is not set -# CONFIG_EXAMPLES_NSH is not set -# CONFIG_EXAMPLES_NULL is not set -# CONFIG_EXAMPLES_NX is not set -# CONFIG_EXAMPLES_NXCONSOLE is not set -# CONFIG_EXAMPLES_NXFFS is not set -# CONFIG_EXAMPLES_NXFLAT is not set -# CONFIG_EXAMPLES_NXHELLO is not set -# CONFIG_EXAMPLES_NXIMAGE is not set -# CONFIG_EXAMPLES_NXLINES is not set -# CONFIG_EXAMPLES_NXTEXT is not set -# CONFIG_EXAMPLES_OSTEST is not set -# CONFIG_EXAMPLES_PASHELLO is not set -# CONFIG_EXAMPLES_PIPE is not set -# CONFIG_EXAMPLES_POLL is not set -# CONFIG_EXAMPLES_POSIXSPAWN is not set -# CONFIG_EXAMPLES_QENCODER is not set -# CONFIG_EXAMPLES_RGMP is not set -# CONFIG_EXAMPLES_ROMFS is not set -# CONFIG_EXAMPLES_SENDMAIL is not set -# CONFIG_EXAMPLES_SERLOOP is not set -# CONFIG_EXAMPLES_SLCD is not set -# CONFIG_EXAMPLES_SMART is not set -# CONFIG_EXAMPLES_TCPECHO is not set -# CONFIG_EXAMPLES_TELNETD is not set -# CONFIG_EXAMPLES_THTTPD is not set -# CONFIG_EXAMPLES_TIFF is not set -# CONFIG_EXAMPLES_TOUCHSCREEN is not set -# CONFIG_EXAMPLES_UDP is not set -# CONFIG_EXAMPLES_UIP is not set -# CONFIG_EXAMPLES_USBSERIAL is not set -# CONFIG_EXAMPLES_USBMSC is not set -# CONFIG_EXAMPLES_USBTERM is not set -# CONFIG_EXAMPLES_WATCHDOG is not set - -# -# Graphics Support -# -# CONFIG_TIFF is not set - -# -# Interpreters -# -# CONFIG_INTERPRETERS_FICL is not set -# CONFIG_INTERPRETERS_PCODE is not set - -# -# Network Utilities -# - -# -# Networking Utilities -# -# CONFIG_NETUTILS_CODECS is not set -# CONFIG_NETUTILS_DHCPC is not set -# CONFIG_NETUTILS_DHCPD is not set -# CONFIG_NETUTILS_FTPC is not set -# CONFIG_NETUTILS_FTPD is not set -# CONFIG_NETUTILS_JSON is not set -# CONFIG_NETUTILS_RESOLV is not set -# CONFIG_NETUTILS_SMTP is not set -# CONFIG_NETUTILS_TELNETD is not set -# CONFIG_NETUTILS_TFTPC is not set -# CONFIG_NETUTILS_THTTPD is not set -# CONFIG_NETUTILS_UIPLIB is not set -# CONFIG_NETUTILS_WEBCLIENT is not set - -# -# FreeModBus -# -# CONFIG_MODBUS is not set - -# -# NSH Library -# -# CONFIG_NSH_LIBRARY is not set - -# -# NxWidgets/NxWM -# - -# -# System NSH Add-Ons -# - -# -# Custom Free Memory Command -# -# CONFIG_SYSTEM_FREE is not set - -# -# I2C tool -# - -# -# FLASH Program Installation -# -# CONFIG_SYSTEM_INSTALL is not set - -# -# FLASH Erase-all Command -# - -# -# readline() -# -# CONFIG_SYSTEM_READLINE is not set +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=0 -# -# Power Off -# -# CONFIG_SYSTEM_POWEROFF is not set # -# RAMTRON -# -# CONFIG_SYSTEM_RAMTRON is not set - +# Settings for apps/nshlib # -# SD Card +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). # -# CONFIG_SYSTEM_SDCARD is not set -# -# Sysinfo -# -# CONFIG_SYSTEM_SYSINFO is not set - -# -# USB Monitor -# +# Disable NSH completely +CONFIG_NSH_CONSOLE=n # -# PX4IO specific driver settings -# -# CONFIG_HRT_TIMER -# Enables the high-resolution timer. The board definition must -# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ -# compare channels to be used. -# CONFIG_HRT_PPM -# Enables R/C PPM input using the HRT. The board definition must -# set HRT_PPM_CHANNEL to the timer capture/compare channel to be -# used, and define GPIO_PPM_IN to configure the appropriate timer -# GPIO. -# CONFIG_PWM_SERVO -# Enables the PWM servo driver. The driver configuration must be -# supplied by the board support at initialisation time. -# Note that USART2 must be disabled on the PX4 board for this to -# be available. -# +# Stack and heap information # -CONFIG_HRT_TIMER=y -CONFIG_HRT_PPM=y - +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=1200 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= From 70549075166427486e5b6588ec163b19633e2a2d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 12:48:58 +1100 Subject: [PATCH 270/293] px4io: updated from upstream keeping small changes for APM --- src/drivers/px4io/px4io.cpp | 390 ++++++++++++++++++--------- src/drivers/px4io/px4io_i2c.cpp | 28 +- src/drivers/px4io/px4io_params.c | 11 + src/drivers/px4io/px4io_serial.cpp | 109 +++++--- src/drivers/px4io/px4io_uploader.cpp | 71 +++-- 5 files changed, 423 insertions(+), 186 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d1c3f1c6c0cd..0b242cc0d790 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -99,8 +99,7 @@ #include "modules/dataman/dataman.h" -extern device::Device *PX4IO_i2c_interface() weak_function; -extern device::Device *PX4IO_serial_interface() weak_function; +#include "px4io_driver.h" #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) @@ -228,7 +227,8 @@ class PX4IO : public device::CDev * * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled */ - inline void set_dsm_vcc_ctl(bool enable) { + inline void set_dsm_vcc_ctl(bool enable) + { _dsm_vcc_ctl = enable; }; @@ -237,7 +237,8 @@ class PX4IO : public device::CDev * * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled */ - inline bool get_dsm_vcc_ctl() { + inline bool get_dsm_vcc_ctl() + { return _dsm_vcc_ctl; }; #endif @@ -560,11 +561,13 @@ PX4IO::~PX4IO() } /* well, kill it anyway, though this will probably crash */ - if (_task != -1) + if (_task != -1) { task_delete(_task); + } - if (_interface != nullptr) + if (_interface != nullptr) { delete _interface; + } /* deallocate perfs */ perf_free(_perf_update); @@ -584,8 +587,9 @@ PX4IO::detect() /* do regular cdev init */ ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); @@ -609,7 +613,8 @@ PX4IO::detect() } int -PX4IO::init(bool rc_handling_disabled) { +PX4IO::init(bool rc_handling_disabled) +{ _rc_handling_disabled = rc_handling_disabled; return init(); } @@ -632,8 +637,9 @@ PX4IO::init() /* do regular cdev init */ ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } /* get some parameters */ unsigned protocol; @@ -672,14 +678,13 @@ PX4IO::init() return -1; } - if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) + if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) { _max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS; + } - if (!_rc_handling_disabled) { param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); - } /* * Check for IO flight state - if FMU was flagged to be in @@ -693,8 +698,9 @@ PX4IO::init() /* get IO's last seen FMU state */ ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); - if (ret != OK) + if (ret != OK) { return ret; + } /* * in-air restart is only tried if the IO board reports it is @@ -861,7 +867,7 @@ PX4IO::init() _task = px4_task_spawn_cmd("px4io", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 1800, + 1500, (main_t)&PX4IO::task_main_trampoline, nullptr); @@ -931,11 +937,13 @@ PX4IO::task_main() /* adjust update interval */ if (_update_interval != 0) { - if (_update_interval < UPDATE_INTERVAL_MIN) + if (_update_interval < UPDATE_INTERVAL_MIN) { _update_interval = UPDATE_INTERVAL_MIN; + } - if (_update_interval > 100) + if (_update_interval > 100) { _update_interval = 100; + } orb_set_interval(_t_actuator_controls_0, _update_interval); /* @@ -986,8 +994,9 @@ PX4IO::task_main() orb_check_last = now; /* try to claim the MAVLink log FD */ - if (_mavlink_fd < 0) + if (_mavlink_fd < 0) { _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + } /* check updates on uORB topics and handle it */ bool updated = false; @@ -1028,7 +1037,7 @@ PX4IO::task_main() parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); - int32_t dsm_bind_val = -1; + int32_t dsm_bind_val; param_t dsm_bind_param; /* see if bind parameter has been set, and reset it to -1 */ @@ -1118,6 +1127,52 @@ PX4IO::task_main() } (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask); + + float trim_val; + param_t parm_handle; + + parm_handle = param_find("TRIM_ROLL"); + + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, &trim_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(trim_val)); + } + + parm_handle = param_find("TRIM_PITCH"); + + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, &trim_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(trim_val)); + } + + parm_handle = param_find("TRIM_YAW"); + + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, &trim_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(trim_val)); + } + + /* S.BUS output */ + int sbus_mode; + parm_handle = param_find("PWM_SBUS_MODE"); + + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, &sbus_mode); + + if (sbus_mode == 1) { + /* enable S.BUS 1 */ + (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + + } else if (sbus_mode == 2) { + /* enable S.BUS 2 */ + (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + + } else { + /* disable S.BUS */ + (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, + (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + } + } } } @@ -1131,8 +1186,9 @@ PX4IO::task_main() DEVICE_DEBUG("exiting"); /* clean up the alternate device node */ - if (_primary_pwm_device) + if (_primary_pwm_device) { unregister_driver(PWM_OUTPUT0_DEVICE_PATH); + } /* tell the dtor that we are exiting */ _task = -1; @@ -1162,8 +1218,7 @@ PX4IO::io_set_control_state(unsigned group) bool changed = false; switch (group) { - case 0: - { + case 0: { orb_check(_t_actuator_controls_0, &changed); if (changed) { @@ -1172,8 +1227,8 @@ PX4IO::io_set_control_state(unsigned group) } } break; - case 1: - { + + case 1: { orb_check(_t_actuator_controls_1, &changed); if (changed) { @@ -1181,8 +1236,8 @@ PX4IO::io_set_control_state(unsigned group) } } break; - case 2: - { + + case 2: { orb_check(_t_actuator_controls_2, &changed); if (changed) { @@ -1190,8 +1245,8 @@ PX4IO::io_set_control_state(unsigned group) } } break; - case 3: - { + + case 3: { orb_check(_t_actuator_controls_3, &changed); if (changed) { @@ -1323,8 +1378,9 @@ PX4IO::io_set_rc_config() */ /* fill the mapping with an error condition triggering value */ - for (unsigned i = 0; i < _max_rc_input; i++) + for (unsigned i = 0; i < _max_rc_input; i++) { input_map[i] = UINT8_MAX; + } /* * NOTE: The indices for mapped channels are 1-based @@ -1467,7 +1523,8 @@ PX4IO::io_handle_status(uint16_t status) if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, + PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; @@ -1518,10 +1575,12 @@ PX4IO::dsm_bind_ioctl(int dsmMode) { if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8")); - int ret = ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); + int ret = ioctl(nullptr, DSM_BIND_START, + (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); - if (ret) + if (ret) { mavlink_log_critical(_mavlink_fd, "binding failed."); + } } else { mavlink_log_info(_mavlink_fd, "[IO] system armed, bind request rejected"); @@ -1617,8 +1676,9 @@ PX4IO::io_get_status() * in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); - if (ret != OK) + if (ret != OK) { return ret; + } io_handle_status(regs[0]); io_handle_alarms(regs[1]); @@ -1653,8 +1713,9 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) */ ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); - if (ret != OK) + if (ret != OK) { return ret; + } /* * Get the channel count any any extra channels. This is no more expensive than reading the @@ -1692,9 +1753,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); - if (ret != OK) + if (ret != OK) { return ret; } + } /* last thing set are the actual channel values as 16 bit values */ for (unsigned i = 0; i < channel_count; i++) { @@ -1703,8 +1765,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* get RSSI from input channel */ if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { - int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) / - ((_rssi_pwm_max - _rssi_pwm_min) / 100); + int rssi = ((input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) * 100) / + (_rssi_pwm_max - _rssi_pwm_min); rssi = rssi > 100 ? 100 : rssi; rssi = rssi < 0 ? 0 : rssi; input_rc.rssi = rssi; @@ -1725,8 +1787,9 @@ PX4IO::io_publish_raw_rc() int ret = io_get_raw_rc_input(rc_val); - if (ret != OK) + if (ret != OK) { return ret; + } /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { @@ -1775,12 +1838,14 @@ PX4IO::io_publish_pwm_outputs() uint16_t ctl[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); - if (ret != OK) + if (ret != OK) { return ret; + } /* convert from register format to float */ - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { outputs.output[i] = ctl[i]; + } outputs.noutputs = _max_actuators; @@ -1799,8 +1864,9 @@ PX4IO::io_publish_pwm_outputs() ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status,sizeof(mixer_status)/sizeof(uint16_t)); memcpy(&motor_limits,&mixer_status,sizeof(motor_limits)); - if (ret != OK) + if (ret != OK) { return ret; + } /* publish mixer status */ if(_to_mixer_status == nullptr) { @@ -1861,8 +1927,9 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset) { uint16_t value; - if (io_reg_get(page, offset, &value, 1) != OK) + if (io_reg_get(page, offset, &value, 1) != OK) { return _io_reg_get_error; + } return value; } @@ -1875,8 +1942,9 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t ret = io_reg_get(page, offset, &value, 1); - if (ret != OK) + if (ret != OK) { return ret; + } value &= ~clearbits; value |= setbits; @@ -1947,8 +2015,9 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) do { unsigned count = buflen; - if (count > max_len) + if (count > max_len) { count = max_len; + } if (count > 0) { memcpy(&msg->text[0], buf, count); @@ -2004,12 +2073,12 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } while (buflen > 0); - /* ensure a closing newline */ + int ret; + + /* send the closing newline */ msg->text[0] = '\n'; msg->text[1] = '\0'; - int ret; - for (int i = 0; i < 30; i++) { /* failed, but give it a 2nd shot */ ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); @@ -2021,26 +2090,24 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } } - if (ret) - return ret; + if (ret == 0) { + /* success, exit */ + break; + } retries--; - DEVICE_LOG("mixer sent"); + } while (retries > 0); - } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK))); + if (retries == 0) { + mavlink_and_console_log_info(_mavlink_fd, "[IO] mixer upload fail"); + /* load must have failed for some reason */ + return -EINVAL; - /* check for the mixer-OK flag */ - if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { - mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); - return 0; + } else { + /* all went well, set the mixer ok flag */ + return io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_MIXER_OK); } - - DEVICE_LOG("mixer rejected by IO"); - mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); - - /* load must have failed for some reason */ - return -EINVAL; } void @@ -2115,14 +2182,16 @@ PX4IO::print_status(bool extended_status) printf("actuators"); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i))); + } printf("\n"); printf("servos"); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); + } uint16_t pwm_invert_mask = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE); @@ -2131,13 +2200,22 @@ PX4IO::print_status(bool extended_status) for (unsigned i = 0; i < _max_actuators; i++) { printf("%s", (pwm_invert_mask & (1 << i)) ? "x" : "_"); } - printf("]\n"); + + printf("]"); + + float trim_roll = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL)); + float trim_pitch = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH)); + float trim_yaw = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW)); + + printf(" trims: r: %8.4f p: %8.4f y: %8.4f\n", + (double)trim_roll, (double)trim_pitch, (double)trim_yaw); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); - for (unsigned i = 0; i < raw_inputs; i++) + for (unsigned i = 0; i < raw_inputs; i++) { printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + } printf("\n"); @@ -2163,16 +2241,18 @@ PX4IO::print_status(bool extended_status) printf("mapped R/C inputs 0x%04x", mapped_inputs); for (unsigned i = 0; i < _max_rc_input; i++) { - if (mapped_inputs & (1 << i)) + if (mapped_inputs & (1 << i)) { printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); } + } printf("\n"); uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); printf("ADC inputs"); - for (unsigned i = 0; i < adc_inputs; i++) + for (unsigned i = 0; i < adc_inputs; i++) { printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + } printf("\n"); @@ -2215,8 +2295,9 @@ PX4IO::print_status(bool extended_status) for (unsigned group = 0; group < 4; group++) { printf("controls %u:", group); - for (unsigned i = 0; i < _max_controls; i++) + for (unsigned i = 0; i < _max_controls; i++) { printf(" %d", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); + } printf("\n"); } @@ -2240,13 +2321,15 @@ PX4IO::print_status(bool extended_status) printf("failsafe"); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + } printf("\ndisarmed values"); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); + } printf("\n"); } @@ -2319,9 +2402,12 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_SET_FAILSAFE_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) /* fail with error */ + { return -E2BIG; + } /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); @@ -2338,9 +2424,12 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_SET_DISARMED_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) /* fail with error */ + { return -E2BIG; + } /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); @@ -2357,9 +2446,12 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_SET_MIN_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) /* fail with error */ + { return -E2BIG; + } /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); @@ -2376,9 +2468,12 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_SET_MAX_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) /* fail with error */ + { return -E2BIG; + } /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); @@ -2519,8 +2614,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); - if (*(uint32_t *)arg == _io_reg_get_error) + if (*(uint32_t *)arg == _io_reg_get_error) { ret = -EIO; + } break; } @@ -2530,8 +2626,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) uint32_t bits = (1 << _max_relays) - 1; /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl) + if (_dsm_vcc_ctl) { bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; + } ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); #endif @@ -2579,8 +2676,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); - if (*(uint32_t *)arg == _io_reg_get_error) + if (*(uint32_t *)arg == _io_reg_get_error) { ret = -EIO; + } #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 @@ -2608,8 +2706,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); - if (ret != OK) + if (ret != OK) { break; + } /* if no R/C input, don't try to fetch anything */ if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { @@ -2645,8 +2744,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; case PX4IO_REBOOT_BOOTLOADER: - if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { return -EINVAL; + } /* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */ io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg); @@ -2658,8 +2758,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) /* check IO firmware CRC against passed value */ uint32_t io_crc = 0; ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); - if (ret != OK) + + if (ret != OK) { return ret; + } + if (io_crc != arg) { DEVICE_DEBUG("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); return -EINVAL; @@ -2703,10 +2806,13 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) if (arg == 1) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + } else if (arg == 2) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } else { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, + (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); } break; @@ -2762,8 +2868,9 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len) { unsigned count = len / 2; - if (count > _max_actuators) + if (count > _max_actuators) { count = _max_actuators; + } if (count > 0) { @@ -2771,9 +2878,10 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len) int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); perf_end(_perf_write); - if (ret != OK) + if (ret != OK) { return ret; } + } return count * 2; } @@ -2816,21 +2924,23 @@ get_interface() #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 - /* try for a serial interface */ - if (PX4IO_serial_interface != nullptr) +#ifdef PX4IO_SERIAL_BASE interface = PX4IO_serial_interface(); +#endif - if (interface != nullptr) + if (interface != nullptr) { goto got; + } #endif - /* try for an I2C interface if we haven't got a serial one */ - if (PX4IO_i2c_interface != nullptr) +#ifdef PX4_I2C_OBDEV_PX4IO interface = PX4IO_i2c_interface(); +#endif - if (interface != nullptr) + if (interface != nullptr) { goto got; + } errx(1, "cannot alloc interface"); @@ -2847,8 +2957,9 @@ get_interface() void start(int argc, char *argv[]) { - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(0, "already loaded"); + } /* allocate the interface */ device::Device *interface = get_interface(); @@ -2897,8 +3008,9 @@ start(int argc, char *argv[]) void detect(int argc, char *argv[]) { - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(0, "already loaded"); + } /* allocate the interface */ device::Device *interface = get_interface(); @@ -2906,8 +3018,9 @@ detect(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver alloc failed"); + } int ret = g_dev->detect(); @@ -2935,8 +3048,10 @@ checkcrc(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver alloc failed"); + } + } else { /* its already running, don't kill the driver */ keep_running = true; @@ -2960,7 +3075,9 @@ checkcrc(int argc, char *argv[]) while (true) { uint8_t buf[16]; int n = read(fd, buf, sizeof(buf)); - if (n <= 0) break; + + if (n <= 0) { break; } + fw_crc = crc32part(buf, n, fw_crc); nbytes += n; } @@ -2991,32 +3108,43 @@ bind(int argc, char *argv[]) { int pulses; - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "px4io must be started first"); + } #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - if (!g_dev->get_dsm_vcc_ctl()) + if (!g_dev->get_dsm_vcc_ctl()) { errx(1, "DSM bind feature not enabled"); + } #endif - if (argc < 3) + if (argc < 3) { errx(0, "needs argument, use dsm2, dsmx or dsmx8"); + } - if (!strcmp(argv[2], "dsm2")) + if (!strcmp(argv[2], "dsm2")) { pulses = DSM2_BIND_PULSES; - else if (!strcmp(argv[2], "dsmx")) + + } else if (!strcmp(argv[2], "dsmx")) { pulses = DSMX_BIND_PULSES; - else if (!strcmp(argv[2], "dsmx8")) + + } else if (!strcmp(argv[2], "dsmx8")) { pulses = DSMX8_BIND_PULSES; - else + + } else { errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]); + } + // Test for custom pulse parameter - if (argc > 3) + if (argc > 3) { pulses = atoi(argv[3]); - if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + } + + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { errx(1, "system must not be armed"); + } #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); @@ -3038,23 +3166,29 @@ test(void) fd = open(PX4IO_DEVICE_PATH, O_WRONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed to open device"); + } - if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) { err(1, "failed to get servo count"); + } /* tell IO that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - if (ret != OK) + if (ret != OK) { err(1, "PWM_SERVO_SET_ARM_OK"); + } - if (ioctl(fd, PWM_SERVO_ARM, 0)) + if (ioctl(fd, PWM_SERVO_ARM, 0)) { err(1, "failed to arm servos"); + } struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; warnx("Press CTRL-C or 'c' to abort."); @@ -3064,13 +3198,15 @@ test(void) /* sweep all servos between 1000..2000 */ servo_position_t servos[servo_count]; - for (unsigned i = 0; i < servo_count; i++) + for (unsigned i = 0; i < servo_count; i++) { servos[i] = pwm_value; + } ret = write(fd, servos, sizeof(servos)); - if (ret != (int)sizeof(servos)) + if (ret != (int)sizeof(servos)) { err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + } if (direction > 0) { if (pwm_value < 2000) { @@ -3093,12 +3229,14 @@ test(void) for (unsigned i = 0; i < servo_count; i++) { servo_position_t value; - if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) + if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) { err(1, "error reading PWM servo %d", i); + } - if (value != servos[i]) + if (value != servos[i]) { errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); } + } /* Check if user wants to quit */ char c; @@ -3211,8 +3349,9 @@ lockdown(int argc, char *argv[]) usleep(10000); } - if (hrt_elapsed_time(&start) > timeout) + if (hrt_elapsed_time(&start) > timeout) { errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES."); + } (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1); @@ -3234,17 +3373,21 @@ int px4io_main(int argc, char *argv[]) { /* check for sufficient number of arguments */ - if (argc < 2) + if (argc < 2) { goto out; + } - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { start(argc - 1, argv + 1); + } - if (!strcmp(argv[1], "detect")) + if (!strcmp(argv[1], "detect")) { detect(argc - 1, argv + 1); + } - if (!strcmp(argv[1], "checkcrc")) + if (!strcmp(argv[1], "checkcrc")) { checkcrc(argc - 1, argv + 1); + } if (!strcmp(argv[1], "update")) { @@ -3265,12 +3408,12 @@ px4io_main(int argc, char *argv[]) } else { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - fn[0] = "/etc/extras/px4io-v1_default.bin"; + fn[0] = "/etc/extras/px4io-v1.bin"; fn[1] = "/fs/microsd/px4io1.bin"; fn[2] = "/fs/microsd/px4io.bin"; fn[3] = nullptr; #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - fn[0] = "/etc/extras/px4io-v2_default.bin"; + fn[0] = "/etc/extras/px4io-v2.bin"; fn[1] = "/fs/microsd/px4io2.bin"; fn[2] = "/fs/microsd/px4io.bin"; fn[3] = nullptr; @@ -3308,8 +3451,9 @@ px4io_main(int argc, char *argv[]) } if (!strcmp(argv[1], "iftest")) { - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "can't iftest when started"); + } if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); } @@ -3361,8 +3505,9 @@ px4io_main(int argc, char *argv[]) /* commands below here require a started driver */ - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "not started"); + } if (!strcmp(argv[1], "limit")) { @@ -3471,20 +3616,25 @@ px4io_main(int argc, char *argv[]) !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || !strcmp(argv[1], "rx_sbus") || - !strcmp(argv[1], "rx_ppm")) + !strcmp(argv[1], "rx_ppm")) { errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); + } - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { test(); + } - if (!strcmp(argv[1], "monitor")) + if (!strcmp(argv[1], "monitor")) { monitor(); + } - if (!strcmp(argv[1], "bind")) + if (!strcmp(argv[1], "bind")) { bind(argc, argv); + } - if (!strcmp(argv[1], "lockdown")) + if (!strcmp(argv[1], "lockdown")) { lockdown(argc, argv); + } if (!strcmp(argv[1], "sbus1_out")) { /* we can cheat and call the driver directly, as it diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index 4cceb5cf291a..4574ec732a68 100755 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file px4io_i2c.cpp - * - * I2C interface for PX4IO - */ +/** + * @file px4io_i2c.cpp + * + * I2C interface for PX4IO + */ /* XXX trim includes */ #include @@ -53,9 +53,9 @@ #include -#ifdef PX4_I2C_OBDEV_PX4IO +#include "px4io_driver.h" -device::Device *PX4IO_i2c_interface(); +#ifdef PX4_I2C_OBDEV_PX4IO class PX4IO_I2C : public device::I2C { @@ -94,8 +94,10 @@ PX4IO_I2C::init() int ret; ret = I2C::init(); - if (ret != OK) + + if (ret != OK) { goto out; + } /* XXX really should do something more here */ @@ -133,8 +135,11 @@ PX4IO_I2C::write(unsigned address, void *data, unsigned count) msgv[1].length = 2 * count; int ret = transfer(msgv, 2); - if (ret == OK) + + if (ret == OK) { ret = count; + } + return ret; } @@ -161,8 +166,11 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count) msgv[1].length = 2 * count; int ret = transfer(msgv, 2); - if (ret == OK) + + if (ret == OK) { ret = count; + } + return ret; } diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index 057bcaf15029..51d39e97ba02 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -129,3 +129,14 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0); * @group PWM Outputs */ PARAM_DEFINE_INT32(PWM_MAIN_REV8, 0); + +/** + * Enable S.BUS out + * + * Set to 1 to enable S.BUS version 1 output instead of RSSI. + * + * @min 0 + * @max 1 + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_SBUS_MODE, 0); diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 566d43fcb76e..2c38c0dd3c11 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -31,14 +31,15 @@ * ****************************************************************************/ - /** - * @file px4io_serial.cpp - * - * Serial interface for PX4IO - */ +/** + * @file px4io_serial.cpp + * + * Serial interface for PX4IO + */ /* XXX trim includes */ #include +#include #include #include @@ -68,9 +69,9 @@ #include -#ifdef PX4IO_SERIAL_BASE +#include "px4io_driver.h" -device::Device *PX4IO_serial_interface(); +#ifdef PX4IO_SERIAL_BASE /* serial register accessors */ #define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) @@ -116,10 +117,10 @@ class PX4IO_serial : public device::Device volatile unsigned _rx_dma_status; /** bus-ownership lock */ - sem_t _bus_semaphore; + px4_sem_t _bus_semaphore; /** client-waiting lock/signal */ - sem_t _completion_semaphore; + px4_sem_t _completion_semaphore; /** * Start the transaction with IO and wait for it to complete. @@ -159,7 +160,7 @@ class PX4IO_serial : public device::Device /* do not allow top copying this class */ PX4IO_serial(PX4IO_serial &); - PX4IO_serial& operator = (const PX4IO_serial &); + PX4IO_serial &operator = (const PX4IO_serial &); }; @@ -199,6 +200,7 @@ PX4IO_serial::~PX4IO_serial() stm32_dmastop(_tx_dma); stm32_dmafree(_tx_dma); } + if (_rx_dma != nullptr) { stm32_dmastop(_rx_dma); stm32_dmafree(_rx_dma); @@ -218,8 +220,8 @@ PX4IO_serial::~PX4IO_serial() stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); /* and kill our semaphores */ - sem_destroy(&_completion_semaphore); - sem_destroy(&_bus_semaphore); + px4_sem_destroy(&_completion_semaphore); + px4_sem_destroy(&_bus_semaphore); perf_free(_pc_txns); perf_free(_pc_dmasetup); @@ -232,8 +234,9 @@ PX4IO_serial::~PX4IO_serial() perf_free(_pc_idle); perf_free(_pc_badidle); - if (g_interface == this) + if (g_interface == this) { g_interface = nullptr; + } } int @@ -243,6 +246,7 @@ PX4IO_serial::init() /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); + if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) { return -1; } @@ -277,8 +281,8 @@ PX4IO_serial::init() rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; /* create semaphores */ - sem_init(&_completion_semaphore, 0, 0); - sem_init(&_bus_semaphore, 0, 1); + px4_sem_init(&_completion_semaphore, 0, 0); + px4_sem_init(&_bus_semaphore, 0, 1); /* XXX this could try talking to IO */ @@ -305,19 +309,22 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg) for (;;) { while (!(rSR & USART_SR_TXE)) ; + rDR = 0x55; } + return 0; - case 1: - { + case 1: { unsigned fails = 0; + for (unsigned count = 0;; count++) { uint16_t value = count & 0xffff; - if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) + if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) { fails++; - + } + if (count >= 5000) { lowsyslog("==== test 1 : %u failures ====\n", fails); perf_print_counter(_pc_txns); @@ -333,12 +340,15 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg) count = 0; } } + return 0; } + case 2: lowsyslog("test 2\n"); return 0; } + default: break; } @@ -353,20 +363,24 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count) uint8_t offset = address & 0xff; const uint16_t *values = reinterpret_cast(data); - if (count > PKT_MAX_REGS) + if (count > PKT_MAX_REGS) { return -EINVAL; + } - sem_wait(&_bus_semaphore); + px4_sem_wait(&_bus_semaphore); int result; + for (unsigned retries = 0; retries < 3; retries++) { _dma_buffer.count_code = count | PKT_CODE_WRITE; _dma_buffer.page = page; _dma_buffer.offset = offset; memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count)); - for (unsigned i = count; i < PKT_MAX_REGS; i++) + + for (unsigned i = count; i < PKT_MAX_REGS; i++) { _dma_buffer.regs[i] = 0x55aa; + } /* XXX implement check byte */ @@ -386,13 +400,16 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count) break; } + perf_count(_pc_retries); } - sem_post(&_bus_semaphore); + px4_sem_post(&_bus_semaphore); - if (result == OK) + if (result == OK) { result = count; + } + return result; } @@ -403,12 +420,14 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) uint8_t offset = address & 0xff; uint16_t *values = reinterpret_cast(data); - if (count > PKT_MAX_REGS) + if (count > PKT_MAX_REGS) { return -EINVAL; + } - sem_wait(&_bus_semaphore); + px4_sem_wait(&_bus_semaphore); int result; + for (unsigned retries = 0; retries < 3; retries++) { _dma_buffer.count_code = count | PKT_CODE_READ; @@ -428,14 +447,16 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) result = -EINVAL; perf_count(_pc_protoerrs); - /* compare the received count with the expected count */ + /* compare the received count with the expected count */ + } else if (PKT_COUNT(_dma_buffer) != count) { /* IO returned the wrong number of registers - no point retrying */ result = -EIO; perf_count(_pc_protoerrs); - /* successful read */ + /* successful read */ + } else { /* copy back the result */ @@ -444,13 +465,16 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) break; } + perf_count(_pc_retries); } - sem_post(&_bus_semaphore); + px4_sem_post(&_bus_semaphore); - if (result == OK) + if (result == OK) { result = count; + } + return result; } @@ -517,14 +541,16 @@ PX4IO_serial::_wait_complete() /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); - abstime.tv_nsec += 10*1000*1000; - if (abstime.tv_nsec >= 1000*1000*1000) { + abstime.tv_nsec += 10 * 1000 * 1000; + + if (abstime.tv_nsec >= 1000 * 1000 * 1000) { abstime.tv_sec++; - abstime.tv_nsec -= 1000*1000*1000; + abstime.tv_nsec -= 1000 * 1000 * 1000; } /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ int ret; + for (;;) { ret = sem_timedwait(&_completion_semaphore, &abstime); @@ -539,6 +565,7 @@ PX4IO_serial::_wait_complete() /* check packet CRC - corrupt packet errors mean IO receive CRC error */ uint8_t crc = _dma_buffer.crc; _dma_buffer.crc = 0; + if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { perf_count(_pc_crcerrs); ret = -EIO; @@ -588,6 +615,7 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status) /* check for packet overrun - this will occur after DMA completes */ uint32_t sr = rSR; + if (sr & (USART_SR_ORE | USART_SR_RXNE)) { (void)rDR; status = DMA_STATUS_TEIF; @@ -600,15 +628,17 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status) rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); /* complete now */ - sem_post(&_completion_semaphore); + px4_sem_post(&_completion_semaphore); } } int PX4IO_serial::_interrupt(int irq, void *context) { - if (g_interface != nullptr) + if (g_interface != nullptr) { g_interface->_do_interrupt(); + } + return 0; } @@ -619,10 +649,10 @@ PX4IO_serial::_do_interrupt() (void)rDR; /* read DR to clear status */ if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ - USART_SR_NE | /* noise error - we have lost a byte due to noise */ - USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ - - /* + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + /* * If we are in the process of listening for something, these are all fatal; * abort the DMA with an error. */ @@ -649,6 +679,7 @@ PX4IO_serial::_do_interrupt() /* verify that the received packet is complete */ size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { perf_count(_pc_badidle); diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 027253905f36..70337c5a0c1b 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -126,8 +126,10 @@ PX4IO_Uploader::upload(const char *filenames[]) /* look for the bootloader for 150 ms */ for (int i = 0; i < 15; i++) { ret = sync(); + if (ret == OK) { break; + } else { usleep(10000); } @@ -143,6 +145,7 @@ PX4IO_Uploader::upload(const char *filenames[]) } struct stat st; + if (stat(filename, &st) != 0) { log("Failed to stat %s - %d\n", filename, (int)errno); tcsetattr(_io_fd, TCSANOW, &t_original); @@ -150,6 +153,7 @@ PX4IO_Uploader::upload(const char *filenames[]) _io_fd = -1; return -errno; } + fw_size = st.st_size; if (_fw_fd == -1) { @@ -180,6 +184,7 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret == OK) { if (bl_rev <= BL_REV) { log("found bootloader revision: %d", bl_rev); + } else { log("found unsupported bootloader revision %d, exiting", bl_rev); tcsetattr(_io_fd, TCSANOW, &t_original); @@ -205,6 +210,7 @@ PX4IO_Uploader::upload(const char *filenames[]) if (bl_rev <= 2) { ret = verify_rev2(fw_size); + } else { /* verify rev 3 and higher. Every version *needs* to be verified. */ ret = verify_rev3(fw_size); @@ -240,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[]) // sleep for enough time for the IO chip to boot. This makes // forceupdate more reliably startup IO again after update - up_udelay(100*1000); + up_udelay(100 * 1000); return ret; } @@ -274,12 +280,15 @@ int PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count) { int ret = OK; + while (count--) { ret = recv_byte_with_timeout(p++, 5000); - if (ret != OK) + if (ret != OK) { break; + } } + return ret; } @@ -296,9 +305,11 @@ PX4IO_Uploader::drain() ret = recv_byte_with_timeout(&c, 40); #ifdef UDEBUG + if (ret == OK) { log("discard 0x%02x", c); } + #endif } while (ret == OK); } @@ -309,8 +320,11 @@ PX4IO_Uploader::send(uint8_t c) #ifdef UDEBUG log("send 0x%02x", c); #endif - if (write(_io_fd, &c, 1) != 1) + + if (write(_io_fd, &c, 1) != 1) { return -errno; + } + return OK; } @@ -318,11 +332,15 @@ int PX4IO_Uploader::send(uint8_t *p, unsigned count) { int ret; + while (count--) { ret = send(*p++); - if (ret != OK) + + if (ret != OK) { break; + } } + return ret; } @@ -334,13 +352,15 @@ PX4IO_Uploader::get_sync(unsigned timeout) ret = recv_byte_with_timeout(c, timeout); - if (ret != OK) + if (ret != OK) { return ret; + } ret = recv_byte_with_timeout(c + 1, timeout); - if (ret != OK) + if (ret != OK) { return ret; + } if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) { log("bad sync 0x%02x,0x%02x", c[0], c[1]); @@ -356,8 +376,9 @@ PX4IO_Uploader::sync() drain(); /* complete any pending program operation */ - for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) + for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) { send(0); + } send(PROTO_GET_SYNC); send(PROTO_EOC); @@ -375,8 +396,9 @@ PX4IO_Uploader::get_info(int param, uint32_t &val) ret = recv_bytes((uint8_t *)&val, sizeof(val)); - if (ret != OK) + if (ret != OK) { return ret; + } return get_sync(); } @@ -395,14 +417,17 @@ static int read_with_retry(int fd, void *buf, size_t n) { int ret; uint8_t retries = 0; + do { ret = read(fd, buf, n); } while (ret == -1 && retries++ < 100); + if (retries != 0) { printf("read of %u bytes needed %u retries\n", (unsigned)n, (unsigned)retries); } + return ret; } @@ -415,6 +440,7 @@ PX4IO_Uploader::program(size_t fw_size) size_t sent = 0; file_buf = new uint8_t[PROG_MULTI_MAX]; + if (!file_buf) { log("Can't allocate program buffer"); return -ENOMEM; @@ -430,13 +456,15 @@ PX4IO_Uploader::program(size_t fw_size) while (sent < fw_size) { /* get more bytes to program */ size_t n = fw_size - sent; + if (n > PROG_MULTI_MAX) { n = PROG_MULTI_MAX; } + count = read_with_retry(_fw_fd, file_buf, n); if (count != (ssize_t)n) { - log("firmware read of %u bytes at %u failed -> %d errno %d", + log("firmware read of %u bytes at %u failed -> %d errno %d", (unsigned)n, (unsigned)sent, (int)count, @@ -478,32 +506,37 @@ PX4IO_Uploader::verify_rev2(size_t fw_size) send(PROTO_EOC); ret = get_sync(); - if (ret != OK) + if (ret != OK) { return ret; + } while (sent < fw_size) { /* get more bytes to verify */ size_t n = fw_size - sent; + if (n > sizeof(file_buf)) { n = sizeof(file_buf); } + count = read_with_retry(_fw_fd, file_buf, n); if (count != (ssize_t)n) { - log("firmware read of %u bytes at %u failed -> %d errno %d", + log("firmware read of %u bytes at %u failed -> %d errno %d", (unsigned)n, (unsigned)sent, (int)count, (int)errno); } - if (count == 0) + if (count == 0) { break; + } sent += count; - if (count < 0) + if (count < 0) { return -errno; + } ASSERT((count % 4) == 0); @@ -564,13 +597,15 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) /* read through the firmware file again and calculate the checksum*/ while (bytes_read < fw_size_local) { size_t n = fw_size_local - bytes_read; + if (n > sizeof(file_buf)) { n = sizeof(file_buf); } + count = read_with_retry(_fw_fd, file_buf, n); if (count != (ssize_t)n) { - log("firmware read of %u bytes at %u failed -> %d errno %d", + log("firmware read of %u bytes at %u failed -> %d errno %d", (unsigned)n, (unsigned)bytes_read, (int)count, @@ -581,9 +616,11 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) if (count == 0) { break; } + /* stop if the file cannot be read */ - if (count < 0) + if (count < 0) { return -errno; + } /* calculate crc32 sum */ sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum); @@ -601,7 +638,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) send(PROTO_GET_CRC); send(PROTO_EOC); - ret = recv_bytes((uint8_t*)(&crc), sizeof(crc)); + ret = recv_bytes((uint8_t *)(&crc), sizeof(crc)); if (ret != OK) { log("did not receive CRC checksum"); @@ -621,7 +658,7 @@ int PX4IO_Uploader::reboot() { send(PROTO_REBOOT); - up_udelay(100*1000); // Ensure the farend is in wait for char. + up_udelay(100 * 1000); // Ensure the farend is in wait for char. send(PROTO_EOC); return OK; From a7bc9de56fdfde92c0c77825d6c73453f0d78c5e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 12:49:45 +1100 Subject: [PATCH 271/293] update .gitignore --- .gitignore | 66 ++++++++++++++++++++++++++++-------------------------- 1 file changed, 34 insertions(+), 32 deletions(-) diff --git a/.gitignore b/.gitignore index 61d26d7e7eb9..e762d48bbf1c 100644 --- a/.gitignore +++ b/.gitignore @@ -1,58 +1,60 @@ -*.d !ROMFS/*/*.d !ROMFS/*/*/*.d !ROMFS/*/*/*/*.d +*.d *.dSYM +*.generated.h *.o +*.orig +*.pretty *.pyc *~ .*.swp +.DS_Store .context .cproject -.DS_Store .gdbinit .project +.pydevproject +.ropeproject .settings .swp +.tags +.tags_sorted_by_file +.vagrant .~lock.* +/.gdbinit.fmu +/.gdbinit.io +/Documentation/doxy.log +/Documentation/doxygen*objdb*tmp +/Documentation/html/ +/nuttx-configs/px4io-v1/src/.depend +/nuttx-configs/px4io-v1/src/Make.dep +/nuttx-configs/px4io-v1/src/libboard.a +/nuttx-configs/px4io-v2/src/.depend +/nuttx-configs/px4io-v2/src/Make.dep +/nuttx-configs/px4io-v2/src/libboard.a Archives/* Build/* -build/* -core -cscope.out Firmware.sublime-workspace +Firmware.zip Images/*.bin Images/*.px4 +Tools/sitl_gazebo +build/* +core +cscope.out +makefiles/config_px4fmu-v2_APM.mk +makefiles/nuttx/config_px4fmu-v1_APM.mk +makefiles/nuttx/config_px4fmu-v2_APM.mk mavlink/include/mavlink/v0.9/ -/nuttx-configs/px4io-v2/src/.depend -/nuttx-configs/px4io-v2/src/Make.dep -/nuttx-configs/px4io-v2/src/libboard.a -/nuttx-configs/px4io-v1/src/.depend -/nuttx-configs/px4io-v1/src/Make.dep -/nuttx-configs/px4io-v1/src/libboard.a -/Documentation/doxy.log -/Documentation/html/ -/Documentation/doxygen*objdb*tmp -.tags -tags -.tags_sorted_by_file -.pydevproject -.ropeproject -*.orig +src/lib/dspal +src/lib/matrix src/modules/uORB/topics/* src/platforms/nuttx/px4_messages/* +src/platforms/posix/px4_messages/ +src/platforms/qurt/px4_messages/ src/platforms/ros/px4_messages/* -Firmware.zip +tags unittests/build -*.generated.h -.vagrant -*.pretty xcode -src/platforms/posix/px4_messages/ -src/platforms/qurt/px4_messages/ -makefiles/config_px4fmu-v2_APM.mk -makefiles/nuttx/config_px4fmu-v1_APM.mk -makefiles/nuttx/config_px4fmu-v2_APM.mk -Tools/sitl_gazebo -src/lib/dspal -src/lib/matrix From fd69a3ac63eb225a39895aaac327a886e7223fb7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 13:05:33 +1100 Subject: [PATCH 272/293] param: merged from upstream --- src/modules/systemlib/param/param.c | 95 ++++++++++++++++++++--------- src/modules/systemlib/param/param.h | 41 ++++--------- 2 files changed, 80 insertions(+), 56 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 2c64e32f17eb..f606f20702ab 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -63,6 +63,9 @@ #include "uORB/uORB.h" #include "uORB/topics/parameter_update.h" +#include "px4_parameters.h" + +#include #if 0 # define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) @@ -86,12 +89,11 @@ extern struct param_info_s param_array[]; extern struct param_info_s *param_info_base; extern struct param_info_s *param_info_limit; #else -extern char __param_start, __param_end; -static const struct param_info_s *param_info_base = (struct param_info_s *) &__param_start; -static const struct param_info_s *param_info_limit = (struct param_info_s *) &__param_end; +// FIXME - start and end are reversed +static const struct param_info_s *param_info_base = (const struct param_info_s *) &px4_parameters; #endif -#define param_info_count ((unsigned)(param_info_limit - param_info_base)) +#define param_info_count px4_parameters.param_count /** * Storage for modified parameters. @@ -115,6 +117,13 @@ get_param_info_count(void) if (!param_changed_storage) { size_param_changed_storage_bytes = (param_info_count / bits_per_allocation_unit) + 1; param_changed_storage = calloc(size_param_changed_storage_bytes, 1); + + /* If the allocation fails we need to indicate failure in the + * API by returning PARAM_INVALID + */ + if (param_changed_storage == NULL) { + return 0; + } } return param_info_count; @@ -140,14 +149,14 @@ static param_t param_find_internal(const char *name, bool notification); static void param_lock(void) { - //do {} while (sem_wait(¶m_sem) != 0); + //do {} while (px4_sem_wait(¶m_sem) != 0); } /** unlock the parameter store */ static void param_unlock(void) { - //sem_post(¶m_sem); + //px4_sem_post(¶m_sem); } /** assert that the parameter store is locked */ @@ -166,7 +175,8 @@ param_assert_locked(void) static bool handle_in_range(param_t param) { - return (param < get_param_info_count()); + int count = get_param_info_count(); + return (count && param < count); } /** @@ -247,6 +257,7 @@ param_find_internal(const char *name, bool notification) param_t param; /* perform a linear search of the known parameters */ + for (param = 0; handle_in_range(param); param++) { if (!strcmp(param_info_base[param].name, name)) { if (notification) { @@ -282,14 +293,16 @@ param_count(void) unsigned param_count_used(void) { - // ensure the allocation has been done - get_param_info_count(); unsigned count = 0; - for (unsigned i = 0; i < size_param_changed_storage_bytes; i++) { - for (unsigned j = 0; j < bits_per_allocation_unit; j++) { - if (param_changed_storage[i] & (1 << j)) { - count++; + // ensure the allocation has been done + if (get_param_info_count()) { + + for (unsigned i = 0; i < size_param_changed_storage_bytes; i++) { + for (unsigned j = 0; j < bits_per_allocation_unit; j++) { + if (param_changed_storage[i] & (1 << j)) { + count++; + } } } } @@ -300,7 +313,9 @@ param_count_used(void) param_t param_for_index(unsigned index) { - if (index < get_param_info_count()) { + unsigned count = get_param_info_count(); + + if (count && index < count) { return (param_t)index; } @@ -310,23 +325,24 @@ param_for_index(unsigned index) param_t param_for_used_index(unsigned index) { - if (index < get_param_info_count()) { + int count = get_param_info_count(); - /* walk all params and count */ - int count = 0; + if (count && index < count) { + /* walk all params and count used params */ + unsigned used_count = 0; for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) { - for (unsigned j = 0; j < 8; j++) { + for (unsigned j = 0; j < bits_per_allocation_unit; j++) { if (param_changed_storage[i] & (1 << j)) { /* we found the right used count, * return the param value */ - if (index == count) { - return (param_t)(i * 8 + j); + if (index == used_count) { + return (param_t)(i * bits_per_allocation_unit + j); } - count++; + used_count++; } } } @@ -354,17 +370,17 @@ param_get_used_index(param_t param) } /* walk all params and count, now knowing that it has a valid index */ - int count = 0; + int used_count = 0; for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) { - for (unsigned j = 0; j < 8; j++) { + for (unsigned j = 0; j < bits_per_allocation_unit; j++) { if (param_changed_storage[i] & (1 << j)) { - if ((unsigned)param == i * 8 + j) { - return count; + if ((unsigned)param == i * bits_per_allocation_unit + j) { + return used_count; } - count++; + used_count++; } } } @@ -682,7 +698,7 @@ param_reset_excludes(const char *excludes[], int num_excludes) param_notify_changes(); } -static const char *param_default_file = "/eeprom/parameters"; +static const char *param_default_file = PX4_ROOTFSDIR"/eeprom/parameters"; static char *param_user_file = NULL; int @@ -715,7 +731,7 @@ param_save_default(void) const char *filename = param_get_default_file(); /* write parameters to temp file */ - fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, 0x777); + fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666); if (fd < 0) { warn("failed to open param file: %s", filename); @@ -1021,3 +1037,26 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang func(arg, param); } } + +uint32_t param_hash_check(void) +{ + uint32_t param_hash = 0; + + param_lock(); + + /* compute the CRC32 over all string param names and 4 byte values */ + for (param_t param = 0; handle_in_range(param); param++) { + if (!param_used(param)) { + continue; + } + + const char *name = param_name(param); + const void *val = param_get_value_ptr(param); + param_hash = crc32part((const uint8_t *)name, strlen(name), param_hash); + param_hash = crc32part(val, sizeof(union param_value_u), param_hash); + } + + param_unlock(); + + return param_hash; +} diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 04a37a229a93..f8334361f1b3 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -333,6 +333,13 @@ __EXPORT int param_save_default(void); */ __EXPORT int param_load_default(void); +/** + * Generate the hash of all parameters and their values + * + * @return CRC32 hash of all param_ids and values + */ +__EXPORT uint32_t param_hash_check(void); + /* * Macros creating static parameter definitions. * @@ -346,34 +353,13 @@ __EXPORT int param_load_default(void); */ /** define an int32 parameter */ -#define PARAM_DEFINE_INT32(_name, _default) \ - static const \ - __attribute__((used, section("__param"))) \ - struct param_info_s __param__##_name = { \ - #_name, \ - PARAM_TYPE_INT32, \ - .val.i = _default \ - } +#define PARAM_DEFINE_INT32(_name, _default) /** define a float parameter */ -#define PARAM_DEFINE_FLOAT(_name, _default) \ - static const \ - __attribute__((used, section("__param"))) \ - struct param_info_s __param__##_name = { \ - #_name, \ - PARAM_TYPE_FLOAT, \ - .val.f = _default \ - } +#define PARAM_DEFINE_FLOAT(_name, _default) /** define a parameter that points to a structure */ -#define PARAM_DEFINE_STRUCT(_name, _default) \ - static const \ - __attribute__((used, section("__param"))) \ - struct param_info_s __param__##_name = { \ - #_name, \ - PARAM_TYPE_STRUCT + sizeof(_default), \ - .val.p = &_default \ - } +#define PARAM_DEFINE_STRUCT(_name, _default) /** * Parameter value union. @@ -382,7 +368,6 @@ union param_value_u { void *p; int32_t i; float f; - long long x; }; /** @@ -394,7 +379,7 @@ union param_value_u { struct param_info_s { const char *name -// GCC 4.8 and higher don't implement proper alignment of static data on +// GCC 4.8 and higher don't implement proper alignment of static data on // 64-bit. This means that the 24-byte param_info_s variables are // 16 byte aligned by GCC and that messes up the assumption that // sequential items in the __param segment can be addressed as an array. @@ -405,9 +390,9 @@ struct param_info_s { // The following hack is for GCC >=4.8 only. Clang works fine without // this. #ifdef __PX4_POSIX - __attribute__((aligned(16))); + __attribute__((aligned(16))); #else - ; + ; #endif param_type_t type; union param_value_u val; From 7303d3ea3b68c5f2e115728f72b81bf994b938bf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Aug 2015 12:01:13 +1000 Subject: [PATCH 273/293] px4fmu: allow for GPIO_SET_OUTPUT with initial value this is needed to prevent inadvertent camera trigger when setting up a port --- src/drivers/px4fmu/fmu.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9e1ea99861f0..53b432aa1de7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1664,7 +1664,9 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) gpios |= 3; /* flip the buffer to output mode if required */ - if (GPIO_SET_OUTPUT == function) { + if (GPIO_SET_OUTPUT == function || + GPIO_SET_OUTPUT_LOW == function || + GPIO_SET_OUTPUT_HIGH == function) { stm32_gpiowrite(GPIO_GPIO_DIR, 1); } } @@ -1683,6 +1685,14 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) stm32_configgpio(_gpio_tab[i].output); break; + case GPIO_SET_OUTPUT_LOW: + stm32_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_SET)) | GPIO_OUTPUT_CLEAR); + break; + + case GPIO_SET_OUTPUT_HIGH: + stm32_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_CLEAR)) | GPIO_OUTPUT_SET); + break; + case GPIO_SET_ALT_1: if (_gpio_tab[i].alt != 0) { stm32_configgpio(_gpio_tab[i].alt); @@ -1749,6 +1759,8 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) break; case GPIO_SET_OUTPUT: + case GPIO_SET_OUTPUT_LOW: + case GPIO_SET_OUTPUT_HIGH: case GPIO_SET_INPUT: case GPIO_SET_ALT_1: gpio_set_function(arg, cmd); From 0913e6a5178fb431fff56b1dde34d3d9d9578e25 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 14:05:51 +1100 Subject: [PATCH 274/293] tone_alarm: reverted changes from recent merge these changes break tone_alarm on FMUv2 for ArduPilot --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 328 +++++--------------- 1 file changed, 85 insertions(+), 243 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 1a54e11809d0..de5f06d66a88 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -47,16 +47,16 @@ * From Wikibooks: * * PLAY "[string expression]" - * + * * Used to play notes and a score ... The tones are indicated by letters A through G. - * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) * immediately after the note letter. See this example: - * + * * PLAY "C C# C C#" * * Whitespaces are ignored inside the string expression. There are also codes that - * set the duration, octave and tempo. They are all case-insensitive. PLAY executes - * the commands or notes the order in which they appear in the string. Any indicators + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators * that change the properties are effective for the notes following that indicator. * * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration @@ -66,15 +66,15 @@ * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. - * Remember that C- is equivalent to B. + * Remember that C- is equivalent to B. * < > Changes the current octave respectively down or up one level. * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. - * Pn Causes a silence (pause) for the length of note indicated (same as Ln). - * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. * It can be used for a pause as well. @@ -116,28 +116,11 @@ #include #include -#include - -/* Check that tone alarm and HRT timers are different */ -#if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER) -# if TONE_ALARM_TIMER == HRT_TIMER -# error TONE_ALARM_TIMER and HRT_TIMER must use different timers. -# endif -#endif /* Tone alarm configuration */ -#if TONE_ALARM_TIMER == 1 -# define TONE_ALARM_BASE STM32_TIM1_BASE -# define TONE_ALARM_CLOCK STM32_APB2_TIM1_CLKIN -# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR -# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM1EN -# ifdef CONFIG_STM32_TIM1 -# error Must not set CONFIG_STM32_TIM1 when TONE_ALARM_TIMER is 1 -# endif -#elif TONE_ALARM_TIMER == 2 +#if TONE_ALARM_TIMER == 2 # define TONE_ALARM_BASE STM32_TIM2_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM2_CLKIN -# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM2EN # ifdef CONFIG_STM32_TIM2 # error Must not set CONFIG_STM32_TIM2 when TONE_ALARM_TIMER is 2 @@ -145,7 +128,6 @@ #elif TONE_ALARM_TIMER == 3 # define TONE_ALARM_BASE STM32_TIM3_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM3_CLKIN -# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM3EN # ifdef CONFIG_STM32_TIM3 # error Must not set CONFIG_STM32_TIM3 when TONE_ALARM_TIMER is 3 @@ -153,7 +135,6 @@ #elif TONE_ALARM_TIMER == 4 # define TONE_ALARM_BASE STM32_TIM4_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM4_CLKIN -# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM4EN # ifdef CONFIG_STM32_TIM4 # error Must not set CONFIG_STM32_TIM4 when TONE_ALARM_TIMER is 4 @@ -161,45 +142,33 @@ #elif TONE_ALARM_TIMER == 5 # define TONE_ALARM_BASE STM32_TIM5_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM5_CLKIN -# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR # define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM5EN # ifdef CONFIG_STM32_TIM5 # error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5 # endif -#elif TONE_ALARM_TIMER == 8 -# define TONE_ALARM_BASE STM32_TIM8_BASE -# define TONE_ALARM_CLOCK STM32_APB2_TIM8_CLKIN -# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR -# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM8EN -# ifdef CONFIG_STM32_TIM8 -# error Must not set CONFIG_STM32_TIM8 when TONE_ALARM_TIMER is 8 -# endif #elif TONE_ALARM_TIMER == 9 # define TONE_ALARM_BASE STM32_TIM9_BASE -# define TONE_ALARM_CLOCK STM32_APB2_TIM9_CLKIN -# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR -# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM9EN +# define TONE_ALARM_CLOCK STM32_APB1_TIM9_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM9EN # ifdef CONFIG_STM32_TIM9 # error Must not set CONFIG_STM32_TIM9 when TONE_ALARM_TIMER is 9 # endif #elif TONE_ALARM_TIMER == 10 # define TONE_ALARM_BASE STM32_TIM10_BASE -# define TONE_ALARM_CLOCK STM32_APB2_TIM10_CLKIN -# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR -# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM10EN +# define TONE_ALARM_CLOCK STM32_APB1_TIM10_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM10EN # ifdef CONFIG_STM32_TIM10 # error Must not set CONFIG_STM32_TIM10 when TONE_ALARM_TIMER is 10 # endif #elif TONE_ALARM_TIMER == 11 # define TONE_ALARM_BASE STM32_TIM11_BASE -# define TONE_ALARM_CLOCK STM32_APB2_TIM11_CLKIN -# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR -# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM11EN +# define TONE_ALARM_CLOCK STM32_APB1_TIM11_CLKIN +# define TONE_ALARM_CLOCK_ENABLE RCC_APB1ENR_TIM11EN # ifdef CONFIG_STM32_TIM11 # error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11 # endif #else -# error Must set TONE_ALARM_TIMER to one of the timers between 1 and 11 (inclusive) to use this driver. +# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver. #endif #if TONE_ALARM_CHANNEL == 1 @@ -232,49 +201,24 @@ */ #define REG(_reg) (*(volatile uint32_t *)(TONE_ALARM_BASE + _reg)) -#if TONE_ALARM_TIMER == 1 || TONE_ALARM_TIMER == 8 // Note: If using TIM1 or TIM8, then you are using the ADVANCED timers and NOT the GENERAL TIMERS, therefore different registers -# define rCR1 REG(STM32_ATIM_CR1_OFFSET) -# define rCR2 REG(STM32_ATIM_CR2_OFFSET) -# define rSMCR REG(STM32_ATIM_SMCR_OFFSET) -# define rDIER REG(STM32_ATIM_DIER_OFFSET) -# define rSR REG(STM32_ATIM_SR_OFFSET) -# define rEGR REG(STM32_ATIM_EGR_OFFSET) -# define rCCMR1 REG(STM32_ATIM_CCMR1_OFFSET) -# define rCCMR2 REG(STM32_ATIM_CCMR2_OFFSET) -# define rCCER REG(STM32_ATIM_CCER_OFFSET) -# define rCNT REG(STM32_ATIM_CNT_OFFSET) -# define rPSC REG(STM32_ATIM_PSC_OFFSET) -# define rARR REG(STM32_ATIM_ARR_OFFSET) -# define rRCR REG(STM32_ATIM_RCR_OFFSET) -# define rCCR1 REG(STM32_ATIM_CCR1_OFFSET) -# define rCCR2 REG(STM32_ATIM_CCR2_OFFSET) -# define rCCR3 REG(STM32_ATIM_CCR3_OFFSET) -# define rCCR4 REG(STM32_ATIM_CCR4_OFFSET) -# define rBDTR REG(STM32_ATIM_BDTR_OFFSET) -# define rDCR REG(STM32_ATIM_DCR_OFFSET) -# define rDMAR REG(STM32_ATIM_DMAR_OFFSET) -#else -# define rCR1 REG(STM32_GTIM_CR1_OFFSET) -# define rCR2 REG(STM32_GTIM_CR2_OFFSET) -# define rSMCR REG(STM32_GTIM_SMCR_OFFSET) -# define rDIER REG(STM32_GTIM_DIER_OFFSET) -# define rSR REG(STM32_GTIM_SR_OFFSET) -# define rEGR REG(STM32_GTIM_EGR_OFFSET) -# define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) -# define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) -# define rCCER REG(STM32_GTIM_CCER_OFFSET) -# define rCNT REG(STM32_GTIM_CNT_OFFSET) -# define rPSC REG(STM32_GTIM_PSC_OFFSET) -# define rARR REG(STM32_GTIM_ARR_OFFSET) -# define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) -# define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) -# define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) -# define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) -# define rDCR REG(STM32_GTIM_DCR_OFFSET) -# define rDMAR REG(STM32_GTIM_DMAR_OFFSET) -#endif - -#define CBRK_BUZZER_KEY 782097 +#define rCR1 REG(STM32_GTIM_CR1_OFFSET) +#define rCR2 REG(STM32_GTIM_CR2_OFFSET) +#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +#define rDIER REG(STM32_GTIM_DIER_OFFSET) +#define rSR REG(STM32_GTIM_SR_OFFSET) +#define rEGR REG(STM32_GTIM_EGR_OFFSET) +#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +#define rCCER REG(STM32_GTIM_CCER_OFFSET) +#define rCNT REG(STM32_GTIM_CNT_OFFSET) +#define rPSC REG(STM32_GTIM_PSC_OFFSET) +#define rARR REG(STM32_GTIM_ARR_OFFSET) +#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +#define rDCR REG(STM32_GTIM_DCR_OFFSET) +#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) class ToneAlarm : public device::CDev { @@ -286,21 +230,14 @@ class ToneAlarm : public device::CDev virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); - inline const char *name(int tune) - { + inline const char *name(int tune) { return _tune_names[tune]; } - enum { - CBRK_OFF = 0, - CBRK_ON, - CBRK_UNINIT - }; - private: static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes - const char *_default_tunes[TONE_NUMBER_OF_TUNES]; - const char *_tune_names[TONE_NUMBER_OF_TUNES]; + const char * _default_tunes[TONE_NUMBER_OF_TUNES]; + const char * _tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -316,7 +253,6 @@ class ToneAlarm : public device::CDev unsigned _octave; unsigned _silence_length; // if nonzero, silence before next note bool _repeat; // if true, tune restarts at end - int _cbrk; //if true, no audio output hrt_call _note_call; // HRT callout for note completion @@ -325,8 +261,8 @@ class ToneAlarm : public device::CDev // unsigned note_to_divisor(unsigned note); - // Calculate the duration in microseconds of play and silence for a - // note given the current tempo, length and mode and the number of + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of // dots following in the play string. // unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); @@ -385,8 +321,7 @@ ToneAlarm::ToneAlarm() : _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), - _next(nullptr), - _cbrk(CBRK_UNINIT) + _next(nullptr) { // enable debug() calls //_debug_enabled = true; @@ -404,7 +339,6 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning _default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning _default_tunes[TONE_SINGLE_BEEP_TUNE] = "MFT100a8"; // single beep - _default_tunes[TONE_HOME_SET] = "MFT100L4>G#6A#6B#4"; _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone @@ -420,7 +354,6 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning _tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning _tune_names[TONE_SINGLE_BEEP_TUNE] = "beep"; // single beep - _tune_names[TONE_HOME_SET] = "home_set"; } ToneAlarm::~ToneAlarm() @@ -434,15 +367,14 @@ ToneAlarm::init() ret = CDev::init(); - if (ret != OK) { + if (ret != OK) return ret; - } /* configure the GPIO to the idle state */ stm32_configgpio(GPIO_TONE_ALARM_IDLE); /* clock/power on our timer */ - modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 0, TONE_ALARM_CLOCK_ENABLE); + modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); /* initialise the timer */ rCR1 = 0; @@ -455,10 +387,6 @@ ToneAlarm::init() rCCER = TONE_CCER; rDCR = 0; -#ifdef rBDTR // If using an advanced timer, you need to activate the output - rBDTR = ATIM_BDTR_MOE; // enable the main output of the advanced timer -#endif - /* toggle the CC output each time the count passes 1 */ TONE_rCCR = 1; @@ -491,31 +419,25 @@ ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (note_length == 0) { + if (note_length == 0) note_length = 1; - } - unsigned note_period = whole_note_period / note_length; switch (_note_mode) { case MODE_NORMAL: silence = note_period / 8; break; - case MODE_STACCATO: silence = note_period / 4; break; - default: case MODE_LEGATO: silence = 0; break; } - note_period -= silence; unsigned dot_extension = note_period / 2; - while (dots--) { note_period += dot_extension; dot_extension /= 2; @@ -529,14 +451,12 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (rest_length == 0) { + if (rest_length == 0) rest_length = 1; - } unsigned rest_period = whole_note_period / rest_length; unsigned dot_extension = rest_period / 2; - while (dots--) { rest_period += dot_extension; dot_extension /= 2; @@ -548,13 +468,6 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) void ToneAlarm::start_note(unsigned note) { - // check if circuit breaker is enabled - if (_cbrk == CBRK_UNINIT) { - _cbrk = circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY); - } - - if (_cbrk != CBRK_OFF) { return; } - // compute the divisor unsigned divisor = note_to_divisor(note); @@ -633,155 +546,115 @@ ToneAlarm::next_note() while (note == 0) { // we always need at least one character from the string int c = next_char(); - - if (c == 0) { + if (c == 0) goto tune_end; - } - _next++; switch (c) { case 'L': // select note length _note_length = next_number(); - - if (_note_length < 1) { + if (_note_length < 1) goto tune_error; - } - break; case 'O': // select octave _octave = next_number(); - - if (_octave > 6) { + if (_octave > 6) _octave = 6; - } - break; case '<': // decrease octave - if (_octave > 0) { + if (_octave > 0) _octave--; - } - break; case '>': // increase octave - if (_octave < 6) { + if (_octave < 6) _octave++; - } - break; case 'M': // select inter-note gap c = next_char(); - - if (c == 0) { + if (c == 0) goto tune_error; - } - _next++; - switch (c) { case 'N': _note_mode = MODE_NORMAL; break; - case 'L': _note_mode = MODE_LEGATO; break; - case 'S': _note_mode = MODE_STACCATO; break; - case 'F': _repeat = false; break; - case 'B': _repeat = true; break; - default: goto tune_error; } - break; case 'P': // pause for a note length stop_note(); - hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(next_number(), next_dots()), - (hrt_callout)next_trampoline, - this); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); return; case 'T': { // change tempo - unsigned nt = next_number(); - - if ((nt >= 32) && (nt <= 255)) { - _tempo = nt; - - } else { - goto tune_error; - } + unsigned nt = next_number(); - break; + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + } else { + goto tune_error; } + break; + } case 'N': // play an arbitrary note note = next_number(); - - if (note > 84) { + if (note > 84) goto tune_error; - } - if (note == 0) { // this is a rest - pause for the current note length hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(_note_length, next_dots()), - (hrt_callout)next_trampoline, - this); - return; + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; } - break; case 'A'...'G': // play a note in the current octave note = _note_tab[c - 'A'] + (_octave * 12) + 1; c = next_char(); - switch (c) { case '#': // up a semitone case '+': - if (note < 84) { + if (note < 84) note++; - } - _next++; break; - case '-': // down a semitone - if (note > 1) { + if (note > 1) note--; - } - _next++; break; - default: // 0 / no next char here is OK break; } - // shorthand length notation note_length = next_number(); - - if (note_length == 0) { + if (note_length == 0) note_length = _note_length; - } - break; default: @@ -807,15 +680,12 @@ ToneAlarm::next_note() // stop (and potentially restart) the tune tune_end: stop_note(); - if (_repeat) { start_tune(_tune); - } else { _tune = nullptr; _default_tune_number = 0; } - return; } @@ -825,7 +695,6 @@ ToneAlarm::next_char() while (isspace(*_next)) { _next++; } - return toupper(*_next); } @@ -837,11 +706,8 @@ ToneAlarm::next_number() for (;;) { c = next_char(); - - if (!isdigit(c)) { + if (!isdigit(c)) return number; - } - _next++; number = (number * 10) + (c - '0'); } @@ -856,7 +722,6 @@ ToneAlarm::next_dots() _next++; dots++; } - return dots; } @@ -890,7 +755,6 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) _next = nullptr; _repeat = false; _default_tune_number = 0; - } else { /* always interrupt alarms, unless they are repeating and already playing */ if (!(_repeat && _default_tune_number == arg)) { @@ -899,7 +763,6 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) start_tune(_default_tunes[arg]); } } - } else { result = -EINVAL; } @@ -914,9 +777,8 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) // irqrestore(flags); /* give it to the superclass if we didn't like it */ - if (result == -ENOTTY) { + if (result == -ENOTTY) result = CDev::ioctl(filp, cmd, arg); - } return result; } @@ -925,9 +787,8 @@ int ToneAlarm::write(file *filp, const char *buffer, size_t len) { // sanity-check the buffer for length and nul-termination - if (len > _tune_max) { + if (len > _tune_max) return -EFBIG; - } // if we have an existing user tune, free it if (_user_tune != nullptr) { @@ -944,16 +805,13 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len) } // if the new tune is empty, we're done - if (buffer[0] == '\0') { + if (buffer[0] == '\0') return OK; - } // allocate a copy of the new tune _user_tune = strndup(buffer, len); - - if (_user_tune == nullptr) { + if (_user_tune == nullptr) return -ENOMEM; - } // and play it start_tune(_user_tune); @@ -976,16 +834,14 @@ play_tune(unsigned tune) fd = open(TONEALARM0_DEVICE_PATH, 0); - if (fd < 0) { + if (fd < 0) err(1, TONEALARM0_DEVICE_PATH); - } ret = ioctl(fd, TONE_SET_ALARM, tune); close(fd); - if (ret != 0) { + if (ret != 0) err(1, "TONE_SET_ALARM"); - } exit(0); } @@ -997,21 +853,17 @@ play_string(const char *str, bool free_buffer) fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY); - if (fd < 0) { + if (fd < 0) err(1, TONEALARM0_DEVICE_PATH); - } ret = write(fd, str, strlen(str) + 1); close(fd); - if (free_buffer) { + if (free_buffer) free((void *)str); - } - if (ret < 0) { + if (ret < 0) err(1, "play tune"); - } - exit(0); } @@ -1026,9 +878,8 @@ tone_alarm_main(int argc, char *argv[]) if (g_dev == nullptr) { g_dev = new ToneAlarm; - if (g_dev == nullptr) { + if (g_dev == nullptr) errx(1, "couldn't allocate the ToneAlarm driver"); - } if (g_dev->init() != OK) { delete g_dev; @@ -1043,39 +894,30 @@ tone_alarm_main(int argc, char *argv[]) play_tune(TONE_STOP_TUNE); } - if (!strcmp(argv1, "stop")) { + if (!strcmp(argv1, "stop")) play_tune(TONE_STOP_TUNE); - } - if ((tune = strtol(argv1, nullptr, 10)) != 0) { + if ((tune = strtol(argv1, nullptr, 10)) != 0) play_tune(tune); - } /* It might be a tune name */ for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) - if (!strcmp(g_dev->name(tune), argv1)) { + if (!strcmp(g_dev->name(tune), argv1)) play_tune(tune); - } /* If it is a file name then load and play it as a string */ if (*argv1 == '/') { FILE *fd = fopen(argv1, "r"); int sz; char *buffer; - - if (fd == nullptr) { + if (fd == nullptr) errx(1, "couldn't open '%s'", argv1); - } - fseek(fd, 0, SEEK_END); sz = ftell(fd); fseek(fd, 0, SEEK_SET); buffer = (char *)malloc(sz + 1); - - if (buffer == nullptr) { + if (buffer == nullptr) errx(1, "not enough memory memory"); - } - fread(buffer, sz, 1, fd); /* terminate the string */ buffer[sz] = 0; From 546f20648ad92d6a3acc03cb4b4a7d0cf09dde21 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Nov 2015 16:17:56 +1100 Subject: [PATCH 275/293] FMUv4: enabled SBUS support based on earlier patches by Lorenz --- nuttx-configs/px4fmu-v4/include/board.h | 4 ++ nuttx-configs/px4fmu-v4/nsh/defconfig | 4 +- src/drivers/boards/px4fmu-v4/board_config.h | 4 +- src/drivers/px4fmu/fmu.cpp | 70 ++++++++++++++++++--- src/drivers/px4fmu/module.mk | 2 + src/lib/rc/dsm.c | 14 +++-- src/lib/rc/dsm.h | 2 +- src/lib/version/version.h | 5 +- src/modules/px4iofirmware/controls.c | 2 +- 9 files changed, 86 insertions(+), 21 deletions(-) diff --git a/nuttx-configs/px4fmu-v4/include/board.h b/nuttx-configs/px4fmu-v4/include/board.h index 54e21bbfe711..2f7c1189f376 100755 --- a/nuttx-configs/px4fmu-v4/include/board.h +++ b/nuttx-configs/px4fmu-v4/include/board.h @@ -215,6 +215,9 @@ #define GPIO_UART4_RX GPIO_UART4_RX_1 #define GPIO_UART4_TX GPIO_UART4_TX_1 +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + #define GPIO_UART7_RX GPIO_UART7_RX_1 #define GPIO_UART7_TX GPIO_UART7_TX_1 @@ -222,6 +225,7 @@ /* UART RX DMA configurations */ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 /* * CAN diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 96cd833d450d..0b653b9eb5be 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -258,7 +258,7 @@ CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y CONFIG_STM32_UART4=y # CONFIG_STM32_UART5 is not set -# CONFIG_STM32_USART6 is not set +CONFIG_STM32_USART6=y CONFIG_STM32_UART7=y CONFIG_STM32_UART8=y # CONFIG_STM32_IWDG is not set @@ -306,7 +306,7 @@ CONFIG_USART3_RXDMA=y CONFIG_UART4_RXDMA=y CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set -# CONFIG_USART6_RXDMA is not set +CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set CONFIG_UART7_RXDMA=y # CONFIG_UART8_RS485 is not set diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 3500d35e613b..ab9449851460 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -214,6 +214,8 @@ __BEGIN_DECLS #define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ #define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) +#define SBUS_SERIAL_PORT "/dev/ttyS4" + /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 #define PWMIN_TIMER_CHANNEL 2 @@ -223,7 +225,7 @@ __BEGIN_DECLS #define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) #define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) -#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) #define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 53b432aa1de7..b0985ccfb809 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -71,6 +71,11 @@ #include #include +#include +#include +#include +#include + #include #include #include @@ -89,7 +94,7 @@ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side */ -#define CONTROL_INPUT_DROP_LIMIT_MS 20 +#define CONTROL_INPUT_DROP_LIMIT_MS 2 #define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev @@ -322,12 +327,9 @@ PX4FMU::PX4FMU() : _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; #endif -#ifdef SBUS_SERIAL_PORT - _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); -#endif - -#ifdef DSM_SERIAL_PORT - _dsm_fd = dsm_init(DSM_SERIAL_PORT); +#ifdef GPIO_SBUS_INV + // this board has a GPIO to control SBUS inversion + stm32_configgpio(GPIO_SBUS_INV); #endif /* only enable this during development */ @@ -643,6 +645,15 @@ PX4FMU::cycle() update_pwm_rev_mask(); +#ifdef SBUS_SERIAL_PORT + _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); +#endif + + #ifdef DSM_SERIAL_PORT + // XXX rather than opening it we need to cycle between protocols until one is locked in + //_dsm_fd = dsm_init(DSM_SERIAL_PORT); + #endif + _initialized = true; } @@ -799,10 +810,46 @@ PX4FMU::cycle() update_pwm_rev_mask(); } + bool rc_updated = false; + +#ifdef SBUS_SERIAL_PORT + bool sbus_failsafe, sbus_frame_drop; + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; + uint16_t raw_rc_count; + bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (sbus_updated) { + // we have a new PPM frame. Publish it. + _rc_in.channel_count = raw_rc_count; + + if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + for (uint8_t i = 0; i < _rc_in.channel_count; i++) { + _rc_in.values[i] = raw_rc_values[i]; + } + + _rc_in.timestamp_publication = hrt_absolute_time(); + _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; + + _rc_in.rc_ppm_frame_length = 0; + _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0; + _rc_in.rc_failsafe = false; + _rc_in.rc_lost = false; + _rc_in.rc_lost_frame_count = 0; + _rc_in.rc_total_frame_count = 0; + + rc_updated = true; + } +#endif + #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data - if (ppm_last_valid_decode != _rc_in.timestamp_last_signal) { + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && + ppm_decoded_channels > 3) { // we have a new PPM frame. Publish it. _rc_in.channel_count = ppm_decoded_channels; @@ -824,6 +871,12 @@ PX4FMU::cycle() _rc_in.rc_lost_frame_count = 0; _rc_in.rc_total_frame_count = 0; + rc_updated = true; + } + +#endif + + if (rc_updated) { /* lazily advertise on first publication */ if (_to_input_rc == nullptr) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in); @@ -833,7 +886,6 @@ PX4FMU::cycle() } } -#endif work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000)); } diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index 5a2806f4b549..273416d85c1e 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -4,6 +4,8 @@ MODULE_COMMAND = fmu SRCS = fmu.cpp \ + ../../lib/rc/sbus.c \ + ../../lib/rc/dsm.c \ px4fmu_params.c MODULE_STACKSIZE = 1200 diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index e1bd7cd09583..7c36e4fc228e 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -68,6 +68,9 @@ static unsigned dsm_partial_frame_count; /**< Count of bytes received for curren static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */ static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */ +static bool +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values); + /** * Attempt to decode a single channel raw channel datum * @@ -265,7 +268,6 @@ void dsm_bind(uint16_t cmd, int pulses) { #if !defined(GPIO_USART1_RX_SPEKTRUM) -#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #else if (dsm_fd < 0) { @@ -332,8 +334,8 @@ dsm_bind(uint16_t cmd, int pulses) * @param[out] num_values pointer to number of raw channel values returned * @return true=DSM frame successfully decoded, false=no update */ -static bool -dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +bool +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values) { /* debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", @@ -379,7 +381,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) } /* ignore channels out of range */ - if (channel >= PX4IO_RC_INPUT_CHANNELS) { + if (channel >= max_values) { continue; } @@ -478,7 +480,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * @return true=decoded raw channel values updated, false=no update */ bool -dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes) +dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values) { ssize_t ret; hrt_abstime now; @@ -528,5 +530,5 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by * decode it. */ dsm_partial_frame_count = 0; - return dsm_decode(now, values, num_values); + return dsm_decode(now, values, num_values, max_values); } diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index 9129a389d8a7..80614d066ead 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -46,7 +46,7 @@ __BEGIN_DECLS __EXPORT int dsm_init(const char *device); -__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); +__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); __EXPORT void dsm_bind(uint16_t cmd, int pulses); __END_DECLS diff --git a/src/lib/version/version.h b/src/lib/version/version.h index e63b1bd6b9db..9492cec90f4b 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -63,7 +63,10 @@ #define HW_ARCH "PX4_STM32F4DISCOVERY" #endif -#ifdef CONFIG_ARCH_BOARD_POSIXTEST +#ifdef CONFIG_ARCH_BOARD_SITL +#define HW_ARCH "LINUXTEST" +#endif +#ifdef CONFIG_ARCH_BOARD_EAGLE #define HW_ARCH "LINUXTEST" #endif #endif /* VERSION_H_ */ diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 2d8e43c8ea45..96a3e8eb4494 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -77,7 +77,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool uint16_t temp_count = r_raw_rc_count; uint8_t n_bytes = 0; uint8_t *bytes; - *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS); if (*dsm_updated) { r_raw_rc_count = temp_count & 0x7fff; From 3692c100d9366392061355c567dbccb59291c7e6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 5 Dec 2015 06:55:52 +1100 Subject: [PATCH 276/293] circuit_breaker: prevent param fetch failure from disabling safety switch if the param get failed then an uninitialised stack variable was used for the safety disable on boot. In ArduPilot that value happened to equal the correct magic due to stack passing from caller. This forced safety off on boot --- src/modules/systemlib/circuit_breaker.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index ba9e9f62197c..c5a94a43bd2d 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -48,9 +48,10 @@ bool circuit_breaker_enabled(const char *breaker, int32_t magic) { - int32_t val; - (void)PX4_PARAM_GET_BYNAME(breaker, &val); - - return (val == magic); + int32_t val = -1; + if (PX4_PARAM_GET_BYNAME(breaker, &val) == 0 && val == magic) { + return true; + } + return false; } From 8048e542599a89acd72fff2fce831a8012201a96 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 16:20:18 +1100 Subject: [PATCH 277/293] mpu6000: work around apparent ICM20608 bug undocumented register 0x11 sometimes starts with value 0, which gives an offset on the Y accel axis of 2.7m/s/s. It sometimes boots with 0xc9, which gives a zero offset. Force it to 0xc9 to get consistently correct behaviour --- src/drivers/mpu6000/mpu6000.cpp | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index a4310265a591..4ef627f8f5bc 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -150,6 +150,15 @@ #define MPU_WHOAMI_6000 0x68 #define ICM_WHOAMI_20608 0xaf +// ICM2608 specific registers + +/* this is an undocumented register which + if set incorrectly results in getting a 2.7m/s/s offset + on the Y axis of the accelerometer +*/ +#define MPUREG_ICM_UNDOC1 0x11 +#define MPUREG_ICM_UNDOC1_VALUE 0xc9 + // Product ID Description for ICM2608 // There is none @@ -296,7 +305,7 @@ class MPU6000 : public device::SPI // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define MPU6000_NUM_CHECKED_REGISTERS 9 +#define MPU6000_NUM_CHECKED_REGISTERS 10 static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -481,7 +490,8 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG }; + MPUREG_INT_PIN_CFG, + MPUREG_ICM_UNDOC1 }; @@ -769,6 +779,10 @@ int MPU6000::reset() write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read usleep(1000); + if (is_icm_device()) { + write_checked_reg(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE); + } + // Oscillator set // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); @@ -1597,6 +1611,12 @@ MPU6000::check_registers(void) the data registers. */ uint8_t v; + + // the MPUREG_ICM_UNDOC1 is specific to the ICM20608 (and undocumented) + if (_checked_registers[_checked_next] == MPUREG_ICM_UNDOC1 && !is_icm_device()) { + _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; + } + if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { /* From 313225f4939afd30b1093311f9aac1a39b355349 Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Fri, 6 Nov 2015 10:43:56 -0800 Subject: [PATCH 278/293] batt_smbus: read button status from the smart battery --- src/drivers/batt_smbus/batt_smbus.cpp | 41 +++++++++++++++++++++------ 1 file changed, 33 insertions(+), 8 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index fb3c524a8224..9321fad8b9f9 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -90,10 +90,12 @@ #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register #define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register #define BATT_SMBUS_MANUFACTURE_NAME 0x20 ///< manufacturer name +#define BATT_SMBUS_MANUFACTURE_DATA 0x23 ///< manufacturer data #define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register #define BATT_SMBUS_CURRENT 0x2a ///< current register -#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz -#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup +#define BATT_SMBUS_MEASUREMENT_INTERVAL_US (1000000 / 10) ///< time in microseconds, measure at 10hz +#define BATT_SMBUS_TIMEOUT_US 10000000 ///< timeout looking for battery 10seconds after startup +#define BATT_SMBUS_BUTTON_DEBOUNCE_MS 300 ///< button holds longer than this time will cause a power off event #define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC @@ -187,6 +189,7 @@ class BATT_SMBUS : public device::I2C orb_id_t _batt_orb_id; ///< uORB battery topic ID uint64_t _start_time; ///< system time we first attempt to communicate with battery uint16_t _batt_capacity; ///< battery's design capacity in mAh (0 means unknown) + uint8_t _button_press_counts; ///< count of button presses detected }; namespace @@ -206,7 +209,8 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : _batt_topic(nullptr), _batt_orb_id(nullptr), _start_time(0), - _batt_capacity(0) + _batt_capacity(0), + _button_press_counts(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -297,8 +301,8 @@ BATT_SMBUS::test() if (updated) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { - warnx("V=%4.2f C=%4.2f DismAh=%4.2f Cap:%d", (float)status.voltage_v, (float)status.current_a, - (float)status.discharged_mah, (int)_batt_capacity); + warnx("V=%4.2f C=%4.2f DismAh=%4.2f Cap:%d Shutdown:%d", (float)status.voltage_v, (float)status.current_a, + (float)status.discharged_mah, (int)_batt_capacity, (int)status.is_powering_off); } } @@ -381,7 +385,7 @@ BATT_SMBUS::cycle() uint64_t now = hrt_absolute_time(); // exit without rescheduling if we have failed to find a battery after 10 seconds - if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) { + if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_US)) { warnx("did not find smart battery"); return; } @@ -403,7 +407,7 @@ BATT_SMBUS::cycle() new_report.voltage_v = ((float)tmp) / 1000.0f; // read current - uint8_t buff[4]; + uint8_t buff[6]; if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | @@ -426,6 +430,27 @@ BATT_SMBUS::cycle() } } + // read the button press indicator + if (read_block(BATT_SMBUS_MANUFACTURE_DATA, buff, 6, false) == 6) { + bool pressed = (buff[1] >> 3) & 0x01; + + if(_button_press_counts >= ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { + // battery will power off + new_report.is_powering_off = true; + // warn only once + if(_button_press_counts++ == ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) { + warnx("system is shutting down NOW..."); + } + } else if(pressed) { + // battery will power off if the button is held + _button_press_counts++; + } else { + // button released early, reset counters + _button_press_counts = 0; + new_report.is_powering_off = false; + } + } + // publish to orb if (_batt_topic != nullptr) { orb_publish(_batt_orb_id, _batt_topic, &new_report); @@ -453,7 +478,7 @@ BATT_SMBUS::cycle() // schedule a fresh cycle call when the measurement is done work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, - USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); + USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_US)); } int From 8c0e006489c792baf41f919403c0059e10086490 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Fri, 16 Oct 2015 18:38:50 -0700 Subject: [PATCH 279/293] msg: add battery_status message --- msg/battery_status.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 4ed56dd03845..f4a8eb06b77c 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -3,3 +3,4 @@ float32 voltage_v # Battery voltage in volts, 0 if unknown float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown float32 current_a # Battery current in amperes, -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown +bool is_powering_off # Power off event imminent indication, false if unknown \ No newline at end of file From c1323c55fa8e2657144a1f2202d7c0492b8acde3 Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Mon, 9 Nov 2015 14:01:45 -0800 Subject: [PATCH 280/293] modules:sensors: initialise battery_status is_powering_off to false --- src/modules/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ab246e14a114..015e7e097da8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2131,6 +2131,7 @@ Sensors::task_main() _battery_status.voltage_filtered_v = -1.0f; _battery_status.current_a = -1.0f; _battery_status.discharged_mah = -1.0f; + _battery_status.is_powering_off = false; /* get a set of initial values */ accel_poll(raw); From 00f787a86ce0edcd2c87d28dd4787fb2869a6827 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Wed, 18 Nov 2015 18:49:44 +1100 Subject: [PATCH 281/293] oreoled: clean-up cycle function, decrease retries also mark unresponsive LEDs are unhealthy and function-ise the XOR checksum'ing --- src/drivers/drv_oreoled.h | 4 +- src/drivers/oreoled/oreoled.cpp | 451 ++++++++++++++++---------------- 2 files changed, 223 insertions(+), 232 deletions(-) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 8be6eb8d1ac2..d036331b56a6 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -101,7 +101,8 @@ #define OREOLED_CMD_READ_LENGTH_MAX 10 /* maximum number of commands retries */ -#define OEROLED_COMMAND_RETRIES 10 +#define OEROLED_COMMAND_RETRIES 2 +#define OEROLED_BOOT_COMMAND_RETRIES 10 /* magic number used to verify the software reset is valid */ #define OEROLED_RESET_NONCE 0x2A @@ -112,7 +113,6 @@ /* microseconds to hold-off between retries */ #define OREOLED_RETRY_HOLDOFF_US 200 -#define OEROLED_BOOT_COMMAND_RETRIES 25 #define OREOLED_BOOT_FLASH_WAITMS 10 #define OREOLED_BOOT_SUPPORTED_VER 0x01 diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index db7038352915..2ce539c9158c 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -75,8 +75,8 @@ #define OREOLED_GENERALCALL_US 4000000U ///< general call sent every 4 seconds #define OREOLED_GENERALCALL_CMD 0x00 ///< general call command sent at regular intervals -#define OREOLED_STARTUP_INTERVAL_US (1000000U / 10U) ///< time in microseconds, measure at 10hz -#define OREOLED_UPDATE_INTERVAL_US (1000000U / 50U) ///< time in microseconds, measure at 10hz +#define OREOLED_STARTUP_INTERVAL_US (1000000U / 10U) ///< time in microseconds, run at 10hz +#define OREOLED_UPDATE_INTERVAL_US (1000000U / 50U) ///< time in microseconds, run at 50hz #define OREOLED_CMD_QUEUE_SIZE 10 ///< up to 10 messages can be queued up to send to the LEDs @@ -122,7 +122,12 @@ class OREOLED : public device::I2C */ void cycle(); + void run_initial_discovery(void); + void run_updates(void); + + void bootloader_update_application(bool force_update); int bootloader_app_reset(int led_num); + int bootloader_app_reset_all(void); int bootloader_app_ping(int led_num); uint16_t bootloader_inapp_checksum(int led_num); int bootloader_ping(int led_num); @@ -131,9 +136,12 @@ class OREOLED : public device::I2C uint16_t bootloader_app_checksum(int led_num); int bootloader_set_colour(int led_num, uint8_t red, uint8_t green); int bootloader_flash(int led_num); + int bootloader_flash_all(bool force_update); int bootloader_boot(int led_num); + int bootloader_boot_all(void); uint16_t bootloader_fw_checksum(void); int bootloader_coerce_healthy(void); + void bootloader_cmd_add_checksum(oreoled_cmd_t* cmd); /* internal variables */ work_s _work; ///< work queue for scheduling reads @@ -306,186 +314,21 @@ OREOLED::cycle() uint64_t now = hrt_absolute_time(); bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_USEC); - /* prepare the response buffer */ - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - /* during startup period keep searching for unhealthy LEDs */ if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) { - /* prepare command to turn off LED */ - /* add two bytes of pre-amble to for higher signal to noise ratio */ - uint8_t msg[] = {0xAA, 0x55, OREOLED_PATTERN_OFF, 0x00}; - - /* attempt to contact each unhealthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (!_healthy[i]) { - perf_begin(_probe_perf); - - /* set I2C address */ - set_address(OREOLED_BASE_I2C_ADDR + i); - - /* Calculate XOR CRC and append to the i2c write data */ - msg[sizeof(msg) - 1] = OREOLED_BASE_I2C_ADDR + i; - - for (uint8_t j = 0; j < sizeof(msg) - 1; j++) { - msg[sizeof(msg) - 1] ^= msg[j]; - } - - /* send I2C command */ - if (transfer(msg, sizeof(msg), reply, 3) == OK) { - if (reply[1] == OREOLED_BASE_I2C_ADDR + i && - reply[2] == msg[sizeof(msg) - 1]) { - DEVICE_LOG("oreoled %u ok - in bootloader", (unsigned)i); - _healthy[i] = true; - _num_healthy++; - - /* If slaves are in application record that so we can reset if we need to bootload */ - /* This additional check is required for LED firmwares below v1.3 and can be - deprecated once all LEDs in the wild have firmware >= v1.3 */ - if(bootloader_ping(i) == OK) { - _in_boot[i] = true; - _num_inboot++; - } - - /* Check for a reply with a checksum offset of 1, - which indicates a response from firmwares >= 1.3 */ - } else if (reply[1] == OREOLED_BASE_I2C_ADDR + i && - reply[2] == msg[sizeof(msg) - 1] + 1) { - DEVICE_LOG("oreoled %u ok - in application", (unsigned)i); - _healthy[i] = true; - _num_healthy++; - - } else { - DEVICE_LOG("oreo reply errors: %u", (unsigned)_reply_errors); - perf_count(_reply_errors); - } - - } else { - perf_count(_comms_errors); - } - - perf_end(_probe_perf); - } - } + run_initial_discovery(); /* schedule another attempt in 0.1 sec */ work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_STARTUP_INTERVAL_US)); return; + } else if (_alwaysupdate || _autoupdate || _num_inboot || _is_ready) { + run_updates(); - } else if (_alwaysupdate) { - /* reset each healthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && !_in_boot[i]) { - /* reset the LED if it's not in the bootloader */ - /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ - bootloader_app_reset(i); - } - } - - /* attempt to update each healthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && _in_boot[i]) { - /* flash the new firmware */ - bootloader_flash(i); - } - } - - /* boot each healthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && _in_boot[i]) { - /* boot the application */ - bootloader_boot(i); - } - } - - /* coerce LEDs with startup issues to be healthy again */ - bootloader_coerce_healthy(); - - /* mandatory updating has finished */ - _alwaysupdate = false; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, - USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); - return; - - } else if (_autoupdate) { - /* check booted oreoleds to see if the app can report it's checksum (release versions >= v1.2) */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && !_in_boot[i]) { - /* put any out of date oreoleds into bootloader mode */ - /* being in bootloader mode signals to be code below that the will likey need updating */ - if (bootloader_inapp_checksum(i) != bootloader_fw_checksum()) { - bootloader_app_reset(i); - } - } - } - - /* reset all healthy oreoleds if the number of outdated oreoled's is > 0 */ - /* this is done for consistency, so if only one oreoled is updating, all LEDs show the same behaviour */ - /* otherwise a single oreoled could appear broken or failed. */ - if (_num_inboot > 0) { - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && !_in_boot[i]) { - /* reset the LED if it's not in the bootloader */ - /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ - bootloader_app_reset(i); - } - } - - /* update each outdated and healthy LED in bootloader mode */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && _in_boot[i]) { - /* only flash LEDs with an old version of the applictioon */ - if (bootloader_app_checksum(i) != bootloader_fw_checksum()) { - /* flash the new firmware */ - bootloader_flash(i); - } - } - } - - /* boot each healthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && _in_boot[i]) { - /* boot the application */ - bootloader_boot(i); - } - } - - /* coerce LEDs with startup issues to be healthy again */ - bootloader_coerce_healthy(); - } - - /* auto updating has finished */ - _autoupdate = false; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, - USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); - return; - - } else if (_num_inboot > 0) { - /* boot any LEDs which are in still in bootloader mode */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_in_boot[i]) { - bootloader_boot(i); - } - } - - /* coerce LEDs with startup issues to be healthy again */ - bootloader_coerce_healthy(); - - /* ensure we don't get stuck in a loop */ - _num_inboot = 0; - - /* schedule a fresh cycle call when the measurement is done */ + /* schedule another attempt in 20mS */ work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); return; - - } else if (!_is_ready) { - /* indicate a ready state since startup has finished */ - _is_ready = true; } /* get next command from queue */ @@ -503,25 +346,23 @@ OREOLED::cycle() /* Calculate XOR CRC and append to the i2c write data */ uint8_t next_cmd_xor = OREOLED_BASE_I2C_ADDR + next_cmd.led_num; - for (uint8_t i = 0; i < next_cmd.num_bytes; i++) { next_cmd_xor ^= next_cmd.buff[i]; } - next_cmd.buff[next_cmd.num_bytes++] = next_cmd_xor; /* send I2C command with a retry limit */ + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 3) == OK) { - if (reply[1] == (OREOLED_BASE_I2C_ADDR + next_cmd.led_num) && reply[2] == next_cmd_xor) { - /* slave returned a valid response */ - break; - - } else { + if (!(reply[1] == (OREOLED_BASE_I2C_ADDR + next_cmd.led_num) && reply[2] == next_cmd_xor)) { + _healthy[next_cmd.led_num] = false; + _num_healthy--; perf_count(_reply_errors); } - } else { + _healthy[next_cmd.led_num] = false; + _num_healthy--; perf_count(_comms_errors); } } @@ -542,6 +383,119 @@ OREOLED::cycle() USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); } +void +OREOLED::run_initial_discovery(void) +{ + /* prepare command to turn off LED */ + /* add two bytes of pre-amble to for higher signal to noise ratio */ + uint8_t msg[] = {0xAA, 0x55, OREOLED_PATTERN_OFF, 0x00}; + + /* attempt to contact each unhealthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (!_healthy[i]) { + perf_begin(_probe_perf); + + /* set I2C address */ + set_address(OREOLED_BASE_I2C_ADDR + i); + + /* Calculate XOR CRC and append to the i2c write data */ + msg[sizeof(msg) - 1] = OREOLED_BASE_I2C_ADDR + i; + + for (uint8_t j = 0; j < sizeof(msg) - 1; j++) { + msg[sizeof(msg) - 1] ^= msg[j]; + } + + /* send I2C command */ + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + if (transfer(msg, sizeof(msg), reply, 3) == OK) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + i && + reply[2] == msg[sizeof(msg) - 1]) { + log("oreoled %u ok - in bootloader", (unsigned)i); + _healthy[i] = true; + _num_healthy++; + + /* If slaves are in application record that so we can reset if we need to bootload */ + /* This additional check is required for LED firmwares below v1.3 and can be + deprecated once all LEDs in the wild have firmware >= v1.3 */ + if(bootloader_ping(i) == OK) { + _in_boot[i] = true; + _num_inboot++; + } + + /* Check for a reply with a checksum offset of 1, + which indicates a response from firmwares >= 1.3 */ + } else if (reply[1] == OREOLED_BASE_I2C_ADDR + i && + reply[2] == msg[sizeof(msg) - 1] + 1) { + log("oreoled %u ok - in application", (unsigned)i); + _healthy[i] = true; + _num_healthy++; + + } else { + log("oreo reply errors: %u", (unsigned)_reply_errors); + perf_count(_reply_errors); + } + + } else { + perf_count(_comms_errors); + } + + perf_end(_probe_perf); + } + } +} + +void +OREOLED::run_updates(void) +{ + if (_alwaysupdate) { + bootloader_update_application(true); + _alwaysupdate = false; + } else if (_autoupdate) { + bootloader_update_application(false); + _autoupdate = false; + } else if (_num_inboot > 0) { + bootloader_boot_all(); + bootloader_coerce_healthy(); + _num_inboot = 0; + } else if (!_is_ready) { + /* indicate a ready state since startup has finished */ + _is_ready = true; + } +} + +void +OREOLED::bootloader_update_application(bool force_update) +{ + /* check booted oreoleds to see if the app can report it's checksum (release versions >= v1.2) */ + if(!force_update) { + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && !_in_boot[i]) { + /* put any out of date oreoleds into bootloader mode */ + /* being in bootloader mode signals to be code below that the will likey need updating */ + if (bootloader_inapp_checksum(i) != bootloader_fw_checksum()) { + bootloader_app_reset(i); + } + } + } + } + + /* reset all healthy oreoleds if the number of outdated oreoled's is > 0 */ + /* this is done for consistency, so if only one oreoled is updating, all LEDs show the same behaviour */ + /* otherwise a single oreoled could appear broken or failed. */ + if (_num_inboot > 0 || force_update) { + bootloader_app_reset_all(); + + /* update each outdated and healthy LED in bootloader mode */ + bootloader_flash_all(force_update); + + /* boot each healthy LED */ + bootloader_boot_all(); + + /* coerce LEDs with startup issues to be healthy again */ + bootloader_coerce_healthy(); + } +} + int OREOLED::bootloader_app_reset(int led_num) { @@ -560,15 +514,12 @@ OREOLED::bootloader_app_reset(int led_num) boot_cmd.buff[2] = OEROLED_RESET_NONCE; boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 4; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } + bootloader_cmd_add_checksum(&boot_cmd); uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; /* send I2C command with a retry limit */ - for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { @@ -592,6 +543,24 @@ OREOLED::bootloader_app_reset(int led_num) return ret; } +int +OREOLED::bootloader_app_reset_all(void) +{ + int ret = OK; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && !_in_boot[i]) { + /* reset the LED if it's not in the bootloader */ + /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ + if (bootloader_app_reset(i) != OK) { + ret = -1; + } + } + } + + return ret; +} + int OREOLED::bootloader_app_ping(int led_num) { @@ -609,15 +578,12 @@ OREOLED::bootloader_app_ping(int led_num) boot_cmd.buff[2] = OREOLED_PATTERN_OFF; boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 4; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } + bootloader_cmd_add_checksum(&boot_cmd); uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; /* send I2C command with a retry limit */ - for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { @@ -647,14 +613,11 @@ OREOLED::bootloader_inapp_checksum(int led_num) boot_cmd.buff[1] = OREOLED_PARAM_APP_CHECKSUM; boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 3; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } + bootloader_cmd_add_checksum(&boot_cmd); uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); @@ -706,10 +669,7 @@ OREOLED::bootloader_ping(int led_num) boot_cmd.buff[0] = OREOLED_BOOT_CMD_PING; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 2; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } + bootloader_cmd_add_checksum(&boot_cmd); uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; @@ -763,10 +723,7 @@ OREOLED::bootloader_version(int led_num) boot_cmd.buff[0] = OREOLED_BOOT_CMD_BL_VER; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 2; - - for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; - } + bootloader_cmd_add_checksum(&boot_cmd); uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; @@ -819,10 +776,7 @@ OREOLED::bootloader_app_version(int led_num) boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_VER; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 2; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } + bootloader_cmd_add_checksum(&boot_cmd); uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; @@ -878,10 +832,7 @@ OREOLED::bootloader_app_checksum(int led_num) boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_CRC; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 2; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } + bootloader_cmd_add_checksum(&boot_cmd); uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; @@ -939,10 +890,7 @@ OREOLED::bootloader_set_colour(int led_num, uint8_t red, uint8_t green) boot_cmd.buff[2] = green; boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 4; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } + bootloader_cmd_add_checksum(&boot_cmd); uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; @@ -1049,10 +997,7 @@ OREOLED::bootloader_flash(int led_num) memcpy(boot_cmd.buff + 2, buf + (page_idx * 64) + OREOLED_FW_FILE_HEADER_LENGTH, 32); boot_cmd.buff[32 + 2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 32 + 3; - - for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; - } + bootloader_cmd_add_checksum(&boot_cmd); for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ @@ -1089,10 +1034,7 @@ OREOLED::bootloader_flash(int led_num) memcpy(boot_cmd.buff + 1, buf + (page_idx * 64) + 32 + OREOLED_FW_FILE_HEADER_LENGTH, 32); boot_cmd.buff[32 + 1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 32 + 2; - - for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; - } + bootloader_cmd_add_checksum(&boot_cmd); for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ @@ -1145,10 +1087,7 @@ OREOLED::bootloader_flash(int led_num) boot_cmd.buff[6] = (uint8_t)(app_checksum & 0xFF); boot_cmd.buff[7] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 8; - - for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; - } + bootloader_cmd_add_checksum(&boot_cmd); /* Try to finalise for twice the amount of normal retries */ for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES * 2; retry > 0; retry--) { @@ -1206,10 +1145,7 @@ OREOLED::bootloader_boot(int led_num) boot_cmd.buff[1] = OREOLED_BOOT_CMD_BOOT_NONCE; boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 3; - - for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; - } + bootloader_cmd_add_checksum(&boot_cmd); for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write */ @@ -1258,6 +1194,22 @@ OREOLED::bootloader_boot(int led_num) return ret; } +int +OREOLED::bootloader_boot_all(void) { + int ret = OK; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && _in_boot[i]) { + /* boot the application */ + if (bootloader_boot(i) != OK) { + ret = -1; + } + } + } + + return ret; +} + uint16_t OREOLED::bootloader_fw_checksum(void) { @@ -1325,6 +1277,31 @@ OREOLED::bootloader_fw_checksum(void) return _fw_checksum; } +int +OREOLED::bootloader_flash_all(bool force_update) +{ + int ret = -1; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && _in_boot[i]) { + int result; + + if (force_update) { + result = bootloader_flash(i); + } else if (bootloader_app_checksum(i) != bootloader_fw_checksum()) { + /* only flash LEDs with an old version of the applictioon */ + result = bootloader_flash(i); + } + + if (result != OK) { + ret = -1; + } + } + } + + return ret; +} + int OREOLED::bootloader_coerce_healthy(void) { @@ -1345,6 +1322,20 @@ OREOLED::bootloader_coerce_healthy(void) return ret; } +void +OREOLED::bootloader_cmd_add_checksum(oreoled_cmd_t* cmd) +{ + if (cmd->num_bytes == 0 || cmd->num_bytes >= OREOLED_CMD_LENGTH_MAX) { + return; + } + + /* write a basic 8-bit XOR checksum into the last byte of the command bytes array*/ + uint8_t checksum_idx = cmd->num_bytes - 1; + for (uint8_t i = 0; i < checksum_idx; i++) { + cmd->buff[checksum_idx] ^= cmd->buff[i]; + } +} + int OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) { From 4a46ce458079fa5ded135dc171de2bd1ad7995ad Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Wed, 18 Nov 2015 23:44:31 +1100 Subject: [PATCH 282/293] oreoled: remove bootloader specific code --- src/drivers/drv_oreoled.h | 56 +- src/drivers/oreoled/oreoled.cpp | 1376 +++---------------------------- 2 files changed, 97 insertions(+), 1335 deletions(-) diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index d036331b56a6..87b02a991c58 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -64,29 +64,8 @@ /** send reset */ #define OREOLED_SEND_RESET _OREOLEDIOC(4) -/** boot ping */ -#define OREOLED_BL_PING _OREOLEDIOC(5) - -/** boot version */ -#define OREOLED_BL_VER _OREOLEDIOC(6) - -/** boot write flash */ -#define OREOLED_BL_FLASH _OREOLEDIOC(7) - -/** boot application version */ -#define OREOLED_BL_APP_VER _OREOLEDIOC(8) - -/** boot application crc */ -#define OREOLED_BL_APP_CRC _OREOLEDIOC(9) - -/** boot startup colour */ -#define OREOLED_BL_SET_COLOUR _OREOLEDIOC(10) - -/** boot application */ -#define OREOLED_BL_BOOT_APP _OREOLEDIOC(11) - /** force an i2c gencall */ -#define OREOLED_FORCE_SYNC _OREOLEDIOC(12) +#define OREOLED_FORCE_SYNC _OREOLEDIOC(5) /* Oreo LED driver supports up to 4 leds */ #define OREOLED_NUM_LEDS 4 @@ -102,39 +81,6 @@ /* maximum number of commands retries */ #define OEROLED_COMMAND_RETRIES 2 -#define OEROLED_BOOT_COMMAND_RETRIES 10 - -/* magic number used to verify the software reset is valid */ -#define OEROLED_RESET_NONCE 0x2A - -/* microseconds to hold-off between write and reads */ -#define OREOLED_WRITE_READ_HOLDOFF_US 500 - -/* microseconds to hold-off between retries */ -#define OREOLED_RETRY_HOLDOFF_US 200 - -#define OREOLED_BOOT_FLASH_WAITMS 10 - -#define OREOLED_BOOT_SUPPORTED_VER 0x01 - -#define OREOLED_BOOT_CMD_PING 0x40 -#define OREOLED_BOOT_CMD_BL_VER 0x41 -#define OREOLED_BOOT_CMD_APP_VER 0x42 -#define OREOLED_BOOT_CMD_APP_CRC 0x43 -#define OREOLED_BOOT_CMD_SET_COLOUR 0x44 - -#define OREOLED_BOOT_CMD_WRITE_FLASH_A 0x50 -#define OREOLED_BOOT_CMD_WRITE_FLASH_B 0x51 -#define OREOLED_BOOT_CMD_FINALISE_FLASH 0x55 - -#define OREOLED_BOOT_CMD_BOOT_APP 0x60 - -#define OREOLED_BOOT_CMD_PING_NONCE 0x2A -#define OREOLED_BOOT_CMD_BOOT_NONCE 0xA2 - -#define OREOLED_FW_FILE_HEADER_LENGTH 2 -#define OREOLED_FW_FILE_SIZE_LIMIT 6144 -#define OREOLED_FW_FILE "/etc/firmware/oreoled.bin" /* enum passed to OREOLED_SET_MODE ioctl() * defined by hardware */ diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 2ce539c9158c..0aff4a741fd1 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -52,8 +52,6 @@ #include #include #include -#include -#include #include #include @@ -71,8 +69,8 @@ #define OREOLED_NUM_LEDS 4 ///< maximum number of LEDs the oreo led driver can support #define OREOLED_BASE_I2C_ADDR 0x68 ///< base i2c address (7-bit) #define OPEOLED_I2C_RETRYCOUNT 2 ///< i2c retry count -#define OREOLED_TIMEOUT_USEC 2000000U ///< timeout looking for oreoleds 2 seconds after startup -#define OREOLED_GENERALCALL_US 4000000U ///< general call sent every 4 seconds +#define OREOLED_TIMEOUT_USEC 1000000U ///< timeout looking for oreoleds 1 second after startup +#define OREOLED_GENERALCALL_US 2000000U ///< general call sent every 2 seconds #define OREOLED_GENERALCALL_CMD 0x00 ///< general call command sent at regular intervals #define OREOLED_STARTUP_INTERVAL_US (1000000U / 10U) ///< time in microseconds, run at 10hz @@ -80,10 +78,13 @@ #define OREOLED_CMD_QUEUE_SIZE 10 ///< up to 10 messages can be queued up to send to the LEDs +/* magic number used to verify the software reset is valid */ +#define OEROLED_RESET_NONCE 0x2A + class OREOLED : public device::I2C { public: - OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate); + OREOLED(int bus, int i2c_addr); virtual ~OREOLED(); virtual int init(); @@ -97,9 +98,6 @@ class OREOLED : public device::I2C /* send cmd to an LEDs (used for testing only) */ int send_cmd(oreoled_cmd_t sb); - /* returns true once the driver finished bootloading and ready for commands */ - bool is_ready(); - private: /** @@ -122,41 +120,17 @@ class OREOLED : public device::I2C */ void cycle(); - void run_initial_discovery(void); - void run_updates(void); - - void bootloader_update_application(bool force_update); - int bootloader_app_reset(int led_num); - int bootloader_app_reset_all(void); - int bootloader_app_ping(int led_num); - uint16_t bootloader_inapp_checksum(int led_num); - int bootloader_ping(int led_num); - uint8_t bootloader_version(int led_num); - uint16_t bootloader_app_version(int led_num); - uint16_t bootloader_app_checksum(int led_num); - int bootloader_set_colour(int led_num, uint8_t red, uint8_t green); - int bootloader_flash(int led_num); - int bootloader_flash_all(bool force_update); - int bootloader_boot(int led_num); - int bootloader_boot_all(void); - uint16_t bootloader_fw_checksum(void); - int bootloader_coerce_healthy(void); - void bootloader_cmd_add_checksum(oreoled_cmd_t* cmd); + void startup_discovery(void); + + uint8_t cmd_add_checksum(oreoled_cmd_t *cmd); /* internal variables */ work_s _work; ///< work queue for scheduling reads bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED - bool _in_boot[OREOLED_NUM_LEDS]; ///< true for each LED that is in bootloader mode uint8_t _num_healthy; ///< number of healthy LEDs ringbuffer::RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs - uint8_t _num_inboot; ///< number of LEDs in bootloader uint64_t _last_gencall; uint64_t _start_time; ///< system time we first attempt to communicate with battery - bool _autoupdate; ///< true if the driver should update all LEDs - bool _alwaysupdate; ///< true if the driver should update all LEDs - bool _is_bootloading; ///< true if a bootloading operation is in progress - bool _is_ready; ///< set to true once the driver has completly initialised - uint16_t _fw_checksum; ///< the current 16bit XOR checksum of the built in oreoled firmware binary /* performance checking */ perf_counter_t _call_perf; @@ -177,18 +151,12 @@ void oreoled_usage(); extern "C" __EXPORT int oreoled_main(int argc, char *argv[]); /* constructor */ -OREOLED::OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate) : +OREOLED::OREOLED(int bus, int i2c_addr) : I2C("oreoled", OREOLED0_DEVICE_PATH, bus, i2c_addr, 100000), _work{}, _num_healthy(0), - _num_inboot(0), _cmd_queue(nullptr), _last_gencall(0), - _autoupdate(autoupdate), - _alwaysupdate(alwaysupdate), - _is_bootloading(false), - _is_ready(false), - _fw_checksum(0x0000), _call_perf(perf_alloc(PC_ELAPSED, "oreoled_call")), _gcall_perf(perf_alloc(PC_ELAPSED, "oreoled_gcall")), _probe_perf(perf_alloc(PC_ELAPSED, "oreoled_probe")), @@ -198,9 +166,6 @@ OREOLED::OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate) : /* initialise to unhealthy */ memset(_healthy, 0, sizeof(_healthy)); - /* initialise to in application */ - memset(_in_boot, 0, sizeof(_in_boot)); - /* capture startup time */ _start_time = hrt_absolute_time(); } @@ -316,1024 +281,123 @@ OREOLED::cycle() /* during startup period keep searching for unhealthy LEDs */ if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) { - run_initial_discovery(); + startup_discovery(); /* schedule another attempt in 0.1 sec */ work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, USEC2TICK(OREOLED_STARTUP_INTERVAL_US)); - return; - } else if (_alwaysupdate || _autoupdate || _num_inboot || _is_ready) { - run_updates(); - - /* schedule another attempt in 20mS */ - work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, - USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); - return; - } - - /* get next command from queue */ - oreoled_cmd_t next_cmd; - - while (_cmd_queue->get(&next_cmd, sizeof(oreoled_cmd_t))) { - /* send valid messages to healthy LEDs */ - if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] - && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { - /* start performance timer */ - perf_begin(_call_perf); - - /* set I2C address */ - set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); - - /* Calculate XOR CRC and append to the i2c write data */ - uint8_t next_cmd_xor = OREOLED_BASE_I2C_ADDR + next_cmd.led_num; - for (uint8_t i = 0; i < next_cmd.num_bytes; i++) { - next_cmd_xor ^= next_cmd.buff[i]; - } - next_cmd.buff[next_cmd.num_bytes++] = next_cmd_xor; - - /* send I2C command with a retry limit */ - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { - if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 3) == OK) { - if (!(reply[1] == (OREOLED_BASE_I2C_ADDR + next_cmd.led_num) && reply[2] == next_cmd_xor)) { - _healthy[next_cmd.led_num] = false; - _num_healthy--; - perf_count(_reply_errors); - } - } else { - _healthy[next_cmd.led_num] = false; - _num_healthy--; - perf_count(_comms_errors); - } - } - - perf_end(_call_perf); - } - } - - /* send general call every 4 seconds, if we aren't bootloading*/ - if (!_is_bootloading && ((now - _last_gencall) > OREOLED_GENERALCALL_US)) { - perf_begin(_gcall_perf); - send_general_call(); - perf_end(_gcall_perf); - } - - /* schedule a fresh cycle call when the command is sent */ - work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, - USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); -} - -void -OREOLED::run_initial_discovery(void) -{ - /* prepare command to turn off LED */ - /* add two bytes of pre-amble to for higher signal to noise ratio */ - uint8_t msg[] = {0xAA, 0x55, OREOLED_PATTERN_OFF, 0x00}; - - /* attempt to contact each unhealthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (!_healthy[i]) { - perf_begin(_probe_perf); - - /* set I2C address */ - set_address(OREOLED_BASE_I2C_ADDR + i); - - /* Calculate XOR CRC and append to the i2c write data */ - msg[sizeof(msg) - 1] = OREOLED_BASE_I2C_ADDR + i; - - for (uint8_t j = 0; j < sizeof(msg) - 1; j++) { - msg[sizeof(msg) - 1] ^= msg[j]; - } - - /* send I2C command */ - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - if (transfer(msg, sizeof(msg), reply, 3) == OK) { - if (reply[1] == OREOLED_BASE_I2C_ADDR + i && - reply[2] == msg[sizeof(msg) - 1]) { - log("oreoled %u ok - in bootloader", (unsigned)i); - _healthy[i] = true; - _num_healthy++; - - /* If slaves are in application record that so we can reset if we need to bootload */ - /* This additional check is required for LED firmwares below v1.3 and can be - deprecated once all LEDs in the wild have firmware >= v1.3 */ - if(bootloader_ping(i) == OK) { - _in_boot[i] = true; - _num_inboot++; - } - - /* Check for a reply with a checksum offset of 1, - which indicates a response from firmwares >= 1.3 */ - } else if (reply[1] == OREOLED_BASE_I2C_ADDR + i && - reply[2] == msg[sizeof(msg) - 1] + 1) { - log("oreoled %u ok - in application", (unsigned)i); - _healthy[i] = true; - _num_healthy++; - - } else { - log("oreo reply errors: %u", (unsigned)_reply_errors); - perf_count(_reply_errors); - } - - } else { - perf_count(_comms_errors); - } - - perf_end(_probe_perf); - } - } -} - -void -OREOLED::run_updates(void) -{ - if (_alwaysupdate) { - bootloader_update_application(true); - _alwaysupdate = false; - } else if (_autoupdate) { - bootloader_update_application(false); - _autoupdate = false; - } else if (_num_inboot > 0) { - bootloader_boot_all(); - bootloader_coerce_healthy(); - _num_inboot = 0; - } else if (!_is_ready) { - /* indicate a ready state since startup has finished */ - _is_ready = true; - } -} - -void -OREOLED::bootloader_update_application(bool force_update) -{ - /* check booted oreoleds to see if the app can report it's checksum (release versions >= v1.2) */ - if(!force_update) { - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && !_in_boot[i]) { - /* put any out of date oreoleds into bootloader mode */ - /* being in bootloader mode signals to be code below that the will likey need updating */ - if (bootloader_inapp_checksum(i) != bootloader_fw_checksum()) { - bootloader_app_reset(i); - } - } - } - } - - /* reset all healthy oreoleds if the number of outdated oreoled's is > 0 */ - /* this is done for consistency, so if only one oreoled is updating, all LEDs show the same behaviour */ - /* otherwise a single oreoled could appear broken or failed. */ - if (_num_inboot > 0 || force_update) { - bootloader_app_reset_all(); - - /* update each outdated and healthy LED in bootloader mode */ - bootloader_flash_all(force_update); - - /* boot each healthy LED */ - bootloader_boot_all(); - - /* coerce LEDs with startup issues to be healthy again */ - bootloader_coerce_healthy(); - } -} - -int -OREOLED::bootloader_app_reset(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - int ret = -1; - - /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - /* send a reset */ - boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; - boot_cmd.buff[1] = OREOLED_PARAM_RESET; - boot_cmd.buff[2] = OEROLED_RESET_NONCE; - boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 4; - bootloader_cmd_add_checksum(&boot_cmd); - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - /* send I2C command with a retry limit */ - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { - if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && - reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - /* slave returned a valid response */ - ret = OK; - /* set this LED as being in boot mode now */ - _in_boot[led_num] = true; - _num_inboot++; - break; - } - } - } - - /* Allow time for the LED to reboot */ - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - - _is_bootloading = false; - return ret; -} - -int -OREOLED::bootloader_app_reset_all(void) -{ - int ret = OK; - - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && !_in_boot[i]) { - /* reset the LED if it's not in the bootloader */ - /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ - if (bootloader_app_reset(i) != OK) { - ret = -1; - } - } - } - - return ret; -} - -int -OREOLED::bootloader_app_ping(int led_num) -{ - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - int ret = -1; - - /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - /* send a pattern off command */ - boot_cmd.buff[0] = 0xAA; - boot_cmd.buff[1] = 0x55; - boot_cmd.buff[2] = OREOLED_PATTERN_OFF; - boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 4; - bootloader_cmd_add_checksum(&boot_cmd); - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - /* send I2C command with a retry limit */ - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { - if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && - reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - /* slave returned a valid response */ - ret = OK; - break; - } - } - } - - return ret; -} - -uint16_t -OREOLED::bootloader_inapp_checksum(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - uint16_t ret = 0x0000; - - /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; - boot_cmd.buff[1] = OREOLED_PARAM_APP_CHECKSUM; - boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 3; - bootloader_cmd_add_checksum(&boot_cmd); - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_PARAM_APP_CHECKSUM && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl app checksum OK from LED %i", boot_cmd.led_num); - warnx("bl app checksum msb: 0x%x", reply[3]); - warnx("bl app checksum lsb: 0x%x", reply[4]); - ret = ((reply[3] << 8) | reply[4]); - break; - - } else { - warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); - warnx("bl app checksum response ADDR: 0x%x", reply[1]); - warnx("bl app checksum response CMD: 0x%x", reply[2]); - warnx("bl app checksum response VER H: 0x%x", reply[3]); - warnx("bl app checksum response VER L: 0x%x", reply[4]); - warnx("bl app checksum response XOR: 0x%x", reply[5]); - - if (retry > 1) { - warnx("bl app checksum retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl app checksum failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -int -OREOLED::bootloader_ping(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - int ret = -1; - - /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_PING; - boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 2; - bootloader_cmd_add_checksum(&boot_cmd); - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_PING && - reply[3] == OREOLED_BOOT_CMD_PING_NONCE && - reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl ping OK from LED %i", boot_cmd.led_num); - ret = OK; - break; - - } else { - warnx("bl ping FAIL from LED %i", boot_cmd.led_num); - warnx("bl ping response ADDR: 0x%x", reply[1]); - warnx("bl ping response CMD: 0x%x", reply[2]); - warnx("bl ping response NONCE: 0x%x", reply[3]); - warnx("bl ping response XOR: 0x%x", reply[4]); - - if (retry > 1) { - warnx("bl ping retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl ping failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -uint8_t -OREOLED::bootloader_version(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - uint8_t ret = 0x00; - - /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_BL_VER; - boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 2; - bootloader_cmd_add_checksum(&boot_cmd); - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_BL_VER && - reply[3] == OREOLED_BOOT_SUPPORTED_VER && - reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl ver from LED %i = %i", boot_cmd.led_num, reply[3]); - ret = reply[3]; - break; - - } else { - warnx("bl ver response ADDR: 0x%x", reply[1]); - warnx("bl ver response CMD: 0x%x", reply[2]); - warnx("bl ver response VER: 0x%x", reply[3]); - warnx("bl ver response XOR: 0x%x", reply[4]); - - if (retry > 1) { - warnx("bl ver retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl ver failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -uint16_t -OREOLED::bootloader_app_version(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - uint16_t ret = 0x0000; - - /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_VER; - boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 2; - bootloader_cmd_add_checksum(&boot_cmd); - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_APP_VER && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl app version OK from LED %i", boot_cmd.led_num); - warnx("bl app version msb: 0x%x", reply[3]); - warnx("bl app version lsb: 0x%x", reply[4]); - ret = ((reply[3] << 8) | reply[4]); - break; - - } else { - warnx("bl app version FAIL from LED %i", boot_cmd.led_num); - warnx("bl app version response ADDR: 0x%x", reply[1]); - warnx("bl app version response CMD: 0x%x", reply[2]); - warnx("bl app version response VER H: 0x%x", reply[3]); - warnx("bl app version response VER L: 0x%x", reply[4]); - warnx("bl app version response XOR: 0x%x", reply[5]); - - if (retry > 1) { - warnx("bl app version retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl app version failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -uint16_t -OREOLED::bootloader_app_checksum(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - uint16_t ret = 0x0000; - - /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_CRC; - boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 2; - bootloader_cmd_add_checksum(&boot_cmd); - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_APP_CRC && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl app checksum OK from LED %i", boot_cmd.led_num); - warnx("bl app checksum msb: 0x%x", reply[3]); - warnx("bl app checksum lsb: 0x%x", reply[4]); - ret = ((reply[3] << 8) | reply[4]); - break; - - } else { - warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); - warnx("bl app checksum response ADDR: 0x%x", reply[1]); - warnx("bl app checksum response CMD: 0x%x", reply[2]); - warnx("bl app checksum response VER H: 0x%x", reply[3]); - warnx("bl app checksum response VER L: 0x%x", reply[4]); - warnx("bl app checksum response XOR: 0x%x", reply[5]); - - if (retry > 1) { - warnx("bl app checksum retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl app checksum failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -int -OREOLED::bootloader_set_colour(int led_num, uint8_t red, uint8_t green) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - int ret = -1; - - /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_SET_COLOUR; - boot_cmd.buff[1] = red; - boot_cmd.buff[2] = green; - boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 4; - bootloader_cmd_add_checksum(&boot_cmd); - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_SET_COLOUR && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl set colour OK from LED %i", boot_cmd.led_num); - ret = OK; - break; - - } else { - warnx("bl set colour FAIL from LED %i", boot_cmd.led_num); - warnx("bl set colour response ADDR: 0x%x", reply[1]); - warnx("bl set colour response CMD: 0x%x", reply[2]); - warnx("bl set colour response XOR: 0x%x", reply[3]); - - if (retry > 1) { - warnx("bl app colour retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl app colour failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -int -OREOLED::bootloader_flash(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - /* Open the bootloader file */ - int fd = ::open(OREOLED_FW_FILE, O_RDONLY); - - /* check for error opening the file */ - if (fd < 0) { - return -1; - } - - struct stat s; - - /* attempt to stat the file */ - if (stat(OREOLED_FW_FILE, &s) != 0) { - ::close(fd); - return -1; - } - - uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; - - /* sanity-check file size */ - if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { - ::close(fd); - return -1; - } - - uint8_t *buf = new uint8_t[s.st_size]; - - /* check that the buffer has been allocated */ - if (buf == NULL) { - ::close(fd); - return -1; - } - - /* check that the firmware can be read into the buffer */ - if (::read(fd, buf, s.st_size) != s.st_size) { - ::close(fd); - delete[] buf; - return -1; - } - - ::close(fd); - - /* Grab the version bytes from the binary */ - uint8_t version_major = buf[0]; - uint8_t version_minor = buf[1]; - - /* calculate flash pages (rounded up to nearest integer) */ - uint8_t flash_pages = ((fw_length + 64 - 1) / 64); - - /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - /* Loop through flash pages */ - for (uint8_t page_idx = 0; page_idx < flash_pages; page_idx++) { - - /* Send the first half of the 64 byte flash page */ - memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); - boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_A; - boot_cmd.buff[1] = page_idx; - memcpy(boot_cmd.buff + 2, buf + (page_idx * 64) + OREOLED_FW_FILE_HEADER_LENGTH, 32); - boot_cmd.buff[32 + 2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 32 + 3; - bootloader_cmd_add_checksum(&boot_cmd); - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_A && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl flash %ia OK for LED %i", page_idx, boot_cmd.led_num); - break; - - } else { - warnx("bl flash %ia FAIL for LED %i", page_idx, boot_cmd.led_num); - warnx("bl flash %ia response ADDR: 0x%x", page_idx, reply[1]); - warnx("bl flash %ia response CMD: 0x%x", page_idx, reply[2]); - warnx("bl flash %ia response XOR: 0x%x", page_idx, reply[3]); - - if (retry > 1) { - warnx("bl flash %ia retrying LED %i", page_idx, boot_cmd.led_num); - - } else { - warnx("bl flash %ia failed on LED %i", page_idx, boot_cmd.led_num); - delete[] buf; - return -1; - } - } - } - - /* Send the second half of the 64 byte flash page */ - memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); - boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_B; - memcpy(boot_cmd.buff + 1, buf + (page_idx * 64) + 32 + OREOLED_FW_FILE_HEADER_LENGTH, 32); - boot_cmd.buff[32 + 1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 32 + 2; - bootloader_cmd_add_checksum(&boot_cmd); - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_B && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl flash %ib OK for LED %i", page_idx, boot_cmd.led_num); - break; - - } else { - warnx("bl flash %ib FAIL for LED %i", page_idx, boot_cmd.led_num); - warnx("bl flash %ib response ADDR: 0x%x", page_idx, reply[1]); - warnx("bl flash %ib response CMD: 0x%x", page_idx, reply[2]); - warnx("bl flash %ib response XOR: 0x%x", page_idx, reply[3]); - - if (retry > 1) { - warnx("bl flash %ib retrying LED %i", page_idx, boot_cmd.led_num); - - } else { - errx(1, "bl flash %ib failed on LED %i", page_idx, boot_cmd.led_num); - delete[] buf; - return -1; - } - } - } - - /* Sleep to allow flash to write */ - /* Wait extra long on the first write, to allow time for EEPROM updates */ - if (page_idx == 0) { - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - - } else { - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000); - } - } - - uint16_t app_checksum = bootloader_fw_checksum(); - - /* Flash writes must have succeeded so finalise the flash */ - boot_cmd.buff[0] = OREOLED_BOOT_CMD_FINALISE_FLASH; - boot_cmd.buff[1] = version_major; - boot_cmd.buff[2] = version_minor; - boot_cmd.buff[3] = (uint8_t)(fw_length >> 8); - boot_cmd.buff[4] = (uint8_t)(fw_length & 0xFF); - boot_cmd.buff[5] = (uint8_t)(app_checksum >> 8); - boot_cmd.buff[6] = (uint8_t)(app_checksum & 0xFF); - boot_cmd.buff[7] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 8; - bootloader_cmd_add_checksum(&boot_cmd); - - /* Try to finalise for twice the amount of normal retries */ - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES * 2; retry > 0; retry--) { - /* Send the I2C Write */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_FINALISE_FLASH && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl finalise OK from LED %i", boot_cmd.led_num); - break; - - } else { - warnx("bl finalise response ADDR: 0x%x", reply[1]); - warnx("bl finalise response CMD: 0x%x", reply[2]); - warnx("bl finalise response XOR: 0x%x", reply[3]); - - if (retry > 1) { - warnx("bl finalise retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl finalise failed on LED %i", boot_cmd.led_num); - delete[] buf; - return -1; - } - } - } - - /* allow time for flash to finalise */ - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - - /* clean up file buffer */ - delete[] buf; - - _is_bootloading = false; - return 1; -} - -int -OREOLED::bootloader_boot(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - int ret = -1; - - /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_BOOT_APP; - boot_cmd.buff[1] = OREOLED_BOOT_CMD_BOOT_NONCE; - boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 3; - bootloader_cmd_add_checksum(&boot_cmd); - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write */ - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_BOOT_APP && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl boot OK from LED %i", boot_cmd.led_num); - /* decrement the inboot counter so we don't get confused */ - _in_boot[led_num] = false; - _num_inboot--; - ret = OK; - break; - - } else if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_BOOT_NONCE && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl boot error from LED %i: no app", boot_cmd.led_num); - break; - - } else { - warnx("bl boot response ADDR: 0x%x", reply[1]); - warnx("bl boot response CMD: 0x%x", reply[2]); - warnx("bl boot response XOR: 0x%x", reply[3]); - - if (retry > 1) { - warnx("bl boot retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl boot failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - /* allow time for the LEDs to boot */ - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - - _is_bootloading = false; - return ret; -} - -int -OREOLED::bootloader_boot_all(void) { - int ret = OK; - - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && _in_boot[i]) { - /* boot the application */ - if (bootloader_boot(i) != OK) { - ret = -1; - } - } - } - - return ret; -} - -uint16_t -OREOLED::bootloader_fw_checksum(void) -{ - /* Calculate the 16 bit XOR checksum of the firmware on the first call of this function */ - if (_fw_checksum == 0x0000) { - /* Open the bootloader file */ - int fd = ::open(OREOLED_FW_FILE, O_RDONLY); - - /* check for error opening the file */ - if (fd < 0) { - return -1; - } - - struct stat s; - - /* attempt to stat the file */ - if (stat(OREOLED_FW_FILE, &s) != 0) { - ::close(fd); - return -1; - } - - uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; - - /* sanity-check file size */ - if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { - ::close(fd); - return -1; - } + return; + } - uint8_t *buf = new uint8_t[s.st_size]; + /* get next command from queue */ + oreoled_cmd_t next_cmd; - /* check that the buffer has been allocated */ - if (buf == NULL) { - ::close(fd); - return -1; - } + while (_cmd_queue->get(&next_cmd, sizeof(oreoled_cmd_t))) { + /* send valid messages to healthy LEDs */ + if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] + && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { + /* start performance timer */ + perf_begin(_call_perf); - /* check that the firmware can be read into the buffer */ - if (::read(fd, buf, s.st_size) != s.st_size) { - ::close(fd); - delete[] buf; - return -1; - } + /* set I2C address */ + set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); - ::close(fd); + /* Calculate XOR checksum and append to the i2c write data */ + uint8_t next_cmd_xor = cmd_add_checksum(&next_cmd); - /* Calculate a 16 bit XOR checksum of the flash */ - /* Skip the first two bytes which are the version information, plus - the next two bytes which are modified by the bootloader */ - uint16_t app_checksum = 0x0000; + /* send I2C command with a retry limit */ + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { + if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 3) == OK) { + if (!(reply[1] == (OREOLED_BASE_I2C_ADDR + next_cmd.led_num) && reply[2] == next_cmd_xor)) { + _healthy[next_cmd.led_num] = false; + _num_healthy--; + perf_count(_reply_errors); + } + } else { + _healthy[next_cmd.led_num] = false; + _num_healthy--; + perf_count(_comms_errors); + } + } - for (uint16_t j = 2 + OREOLED_FW_FILE_HEADER_LENGTH; j < s.st_size; j += 2) { - app_checksum ^= (buf[j] << 8) | buf[j + 1]; + perf_end(_call_perf); } + } - delete[] buf; - - warnx("fw length = %i", fw_length); - warnx("fw checksum = %i", app_checksum); - - /* Store the checksum so it's only calculated once */ - _fw_checksum = app_checksum; + /* send general call every 4 seconds, if we aren't bootloading*/ + if ((now - _last_gencall) > OREOLED_GENERALCALL_US) { + perf_begin(_gcall_perf); + send_general_call(); + perf_end(_gcall_perf); } - return _fw_checksum; + /* schedule a fresh cycle call when the command is sent */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, + USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); } -int -OREOLED::bootloader_flash_all(bool force_update) +void +OREOLED::startup_discovery(void) { - int ret = -1; + oreoled_cmd_t cmd; + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + /* attempt to contact each unhealthy LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && _in_boot[i]) { - int result; - - if (force_update) { - result = bootloader_flash(i); - } else if (bootloader_app_checksum(i) != bootloader_fw_checksum()) { - /* only flash LEDs with an old version of the applictioon */ - result = bootloader_flash(i); - } + if (!_healthy[i]) { + perf_begin(_probe_perf); - if (result != OK) { - ret = -1; - } - } - } + /* prepare command to turn off LED */ + cmd.led_num = i; + cmd.buff[0] = 0xAA; + cmd.buff[1] = 0x55; + cmd.buff[2] = OREOLED_PATTERN_OFF; + cmd.num_bytes = 3; + uint8_t cmd_checksum = cmd_add_checksum(&cmd); - return ret; -} + /* set I2C address */ + set_address(OREOLED_BASE_I2C_ADDR + cmd.led_num); -int -OREOLED::bootloader_coerce_healthy(void) -{ - int ret = -1; + /* send I2C command */ + if (transfer(cmd.buff, cmd.num_bytes, reply, 3) == OK) { + /* Check for a reply with a checksum offset of 1, + which indicates a response from firmwares >= 1.3 */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + cmd.led_num && + reply[2] == (cmd_checksum + 1)) { + log("oreoled %u ok - in application", (unsigned)i); + _healthy[i] = true; + _num_healthy++; + } else { + warnx("startup response ADDR: 0x%x expected 0x%x", reply[1], cmd.led_num); + warnx("startup response CMD: 0x%x expected 0x%x", reply[2], (cmd_checksum + 1)); + perf_count(_reply_errors); + } + } else { + perf_count(_comms_errors); + } - /* check each unhealthy LED */ - /* this re-checks "unhealthy" LEDs as they can sometimes power up with the wrong ID, */ - /* but will have the correct ID once they jump to the application and be healthy again */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (!_healthy[i] && bootloader_app_ping(i) == OK) { - /* mark as healthy */ - _healthy[i] = true; - _num_healthy++; - ret = OK; + perf_end(_probe_perf); } } - - return ret; } -void -OREOLED::bootloader_cmd_add_checksum(oreoled_cmd_t* cmd) +uint8_t +OREOLED::cmd_add_checksum(oreoled_cmd_t *cmd) { - if (cmd->num_bytes == 0 || cmd->num_bytes >= OREOLED_CMD_LENGTH_MAX) { - return; - } - - /* write a basic 8-bit XOR checksum into the last byte of the command bytes array*/ + /* Calculate XOR checksum and append to the command buffer */ + cmd->num_bytes++; uint8_t checksum_idx = cmd->num_bytes - 1; + + /* XOR seed */ + cmd->buff[checksum_idx] = OREOLED_BASE_I2C_ADDR + cmd->led_num; + + /* XOR Calculation */ for (uint8_t i = 0; i < checksum_idx; i++) { cmd->buff[checksum_idx] ^= cmd->buff[i]; } + + return cmd->buff[checksum_idx]; } int @@ -1416,7 +480,6 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { /* add command to queue for all healthy leds */ if (_healthy[i]) { - warnx("sending a reset... to %i", i); new_cmd.led_num = i; _cmd_queue->force(&new_cmd); ret = OK; @@ -1425,80 +488,6 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; - case OREOLED_BL_PING: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_ping(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_VER: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_version(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_FLASH: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_flash(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_APP_VER: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_app_version(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_APP_CRC: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_app_checksum(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_SET_COLOUR: - new_cmd.led_num = OREOLED_ALL_INSTANCES; - - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_set_colour(i, ((oreoled_rgbset_t *) arg)->red, ((oreoled_rgbset_t *) arg)->green); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_BOOT_APP: - new_cmd.led_num = OREOLED_ALL_INSTANCES; - - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_boot(i); - ret = OK; - } - } - - return ret; - case OREOLED_SEND_BYTES: /* send bytes */ new_cmd = *((oreoled_cmd_t *) arg); @@ -1579,18 +568,10 @@ OREOLED::send_cmd(oreoled_cmd_t new_cmd) return ret; } -/* return the internal _is_ready flag indicating if initialisation is complete */ -bool -OREOLED::is_ready() -{ - return _is_ready; -} - void oreoled_usage() { warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'reset', 'rgb 30 40 50', 'macro 4', 'gencall', 'bytes 7 9 6'"); - warnx("bootloader commands: try 'blping', 'blver', 'blappver', 'blappcrc', 'blcolour ', 'blflash', 'blboot'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", OREOLED_BASE_I2C_ADDR); @@ -1641,20 +622,8 @@ oreoled_main(int argc, char *argv[]) i2cdevice = PX4_I2C_BUS_LED; } - /* handle update flags */ - bool autoupdate = false; - bool alwaysupdate = false; - - if (argc > 2 && !strcmp(argv[2], "autoupdate")) { - warnx("autoupdate enabled"); - autoupdate = true; - } else if (argc > 2 && !strcmp(argv[2], "alwaysupdate")) { - warnx("alwaysupdate enabled"); - alwaysupdate = true; - } - /* instantiate driver */ - g_oreoled = new OREOLED(i2cdevice, i2c_addr, autoupdate, alwaysupdate); + g_oreoled = new OREOLED(i2cdevice, i2c_addr); /* check if object was created */ if (g_oreoled == nullptr) { @@ -1668,15 +637,6 @@ oreoled_main(int argc, char *argv[]) errx(1, "failed to start driver"); } - /* wait for up to 20 seconds for the driver become ready */ - for (uint8_t i = 0; i < 20; i++) { - if (g_oreoled != nullptr && g_oreoled->is_ready()) { - break; - } - - sleep(1); - } - exit(0); } @@ -1832,150 +792,6 @@ oreoled_main(int argc, char *argv[]) exit(ret); } - /* attempt to flash all LEDS in bootloader mode*/ - if (!strcmp(verb, "blflash")) { - if (argc < 2) { - errx(1, "Usage: oreoled blflash"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_FLASH, 0)) != OK) { - errx(1, "failed to run flash"); - } - - close(fd); - exit(ret); - } - - /* send bootloader boot request to all LEDS */ - if (!strcmp(verb, "blboot")) { - if (argc < 2) { - errx(1, "Usage: oreoled blboot"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_BOOT_APP, 0)) != OK) { - errx(1, "failed to run boot"); - } - - close(fd); - exit(ret); - } - - /* send bootloader ping all LEDs */ - if (!strcmp(verb, "blping")) { - if (argc < 2) { - errx(1, "Usage: oreoled blping"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_PING, 0)) != OK) { - errx(1, "failed to run blping"); - } - - close(fd); - exit(ret); - } - - /* ask all LEDs for their bootloader version */ - if (!strcmp(verb, "blver")) { - if (argc < 2) { - errx(1, "Usage: oreoled blver"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_VER, 0)) != OK) { - errx(1, "failed to get bootloader version"); - } - - close(fd); - exit(ret); - } - - /* ask all LEDs for their application version */ - if (!strcmp(verb, "blappver")) { - if (argc < 2) { - errx(1, "Usage: oreoled blappver"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_APP_VER, 0)) != OK) { - errx(1, "failed to get boot app version"); - } - - close(fd); - exit(ret); - } - - /* ask all LEDs for their application crc */ - if (!strcmp(verb, "blappcrc")) { - if (argc < 2) { - errx(1, "Usage: oreoled blappcrc"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_APP_CRC, 0)) != OK) { - errx(1, "failed to get boot app crc"); - } - - close(fd); - exit(ret); - } - - /* set the default bootloader LED colour on all LEDs */ - if (!strcmp(verb, "blcolour")) { - if (argc < 4) { - errx(1, "Usage: oreoled blcolour "); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - uint8_t red = (uint8_t)strtol(argv[2], NULL, 0); - uint8_t green = (uint8_t)strtol(argv[3], NULL, 0); - oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, 0}; - - if ((ret = ioctl(fd, OREOLED_BL_SET_COLOUR, (unsigned long)&rgb_set)) != OK) { - errx(1, "failed to set boot startup colours"); - } - - close(fd); - exit(ret); - } - /* send general hardware call to all LEDS */ if (!strcmp(verb, "gencall")) { ret = g_oreoled->send_general_call(); From db10b1134cfdd40bba13915d3c5849aac6ba0fb2 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Thu, 19 Nov 2015 03:06:19 +1100 Subject: [PATCH 283/293] oreoledbl: add separate extensible driver for oreoled bootloading --- makefiles/nuttx/config_px4fmu-v2_default.mk | 1 + src/drivers/drv_oreoled.h | 3 + src/drivers/drv_oreoled_bootloader.h | 77 + src/drivers/oreoled/oreoled.cpp | 2 - .../oreoled/oreoled_bootloader/module.mk | 9 + .../oreoled_bootloader/oreoled_bootloader.cpp | 400 ++++++ .../oreoled_bootloader/oreoled_bootloader.h | 77 + .../oreoled_bootloader_avr.cpp | 1263 +++++++++++++++++ .../oreoled_bootloader_avr.h | 103 ++ 9 files changed, 1933 insertions(+), 2 deletions(-) create mode 100644 src/drivers/drv_oreoled_bootloader.h create mode 100644 src/drivers/oreoled/oreoled_bootloader/module.mk create mode 100644 src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp create mode 100644 src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h create mode 100644 src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp create mode 100644 src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h diff --git a/makefiles/nuttx/config_px4fmu-v2_default.mk b/makefiles/nuttx/config_px4fmu-v2_default.mk index 4c8f60e69393..52b19f948027 100644 --- a/makefiles/nuttx/config_px4fmu-v2_default.mk +++ b/makefiles/nuttx/config_px4fmu-v2_default.mk @@ -44,6 +44,7 @@ MODULES += modules/sensors MODULES += drivers/mkblctrl MODULES += drivers/px4flow MODULES += drivers/oreoled +MODULES += drivers/oreoled/oreoled_bootloader MODULES += drivers/gimbal MODULES += drivers/pwm_input diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 87b02a991c58..4c749001b15e 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -70,6 +70,9 @@ /* Oreo LED driver supports up to 4 leds */ #define OREOLED_NUM_LEDS 4 +/* base i2c address (7-bit) */ +#define OREOLED_BASE_I2C_ADDR 0x68 + /* instance of 0xff means apply to all instances */ #define OREOLED_ALL_INSTANCES 0xff diff --git a/src/drivers/drv_oreoled_bootloader.h b/src/drivers/drv_oreoled_bootloader.h new file mode 100644 index 000000000000..1b06c4cc56e0 --- /dev/null +++ b/src/drivers/drv_oreoled_bootloader.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_oreoled.h + * @author Angus Peart + * OreoLED bootloader device API + */ + +#pragma once + +#include +#include + +/* oreoled device path */ +#define OREOLEDBL0_DEVICE_PATH "/dev/oreoledbl0" + +/* + * ioctl() definitions + */ + +#define _OREOLED_BL_IOC_BASE (0x2f00) +#define _OREOLED_BL_IOC(_n) (_IOC(_OREOLED_BL_IOC_BASE, _n)) + +/** send reset */ +#define OREOLED_BL_RESET _OREOLED_BL_IOC(1) + +/** boot ping */ +#define OREOLED_BL_PING _OREOLED_BL_IOC(2) + +/** boot version */ +#define OREOLED_BL_VER _OREOLED_BL_IOC(3) + +/** boot write flash */ +#define OREOLED_BL_FLASH _OREOLED_BL_IOC(4) + +/** boot application version */ +#define OREOLED_BL_APP_VER _OREOLED_BL_IOC(5) + +/** boot application crc */ +#define OREOLED_BL_APP_CHECKSUM _OREOLED_BL_IOC(6) + +/** boot startup colour */ +#define OREOLED_BL_SET_COLOUR _OREOLED_BL_IOC(7) + +/** boot application */ +#define OREOLED_BL_BOOT_APP _OREOLED_BL_IOC(8) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 0aff4a741fd1..6b2e590959c6 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -66,8 +66,6 @@ #include #include -#define OREOLED_NUM_LEDS 4 ///< maximum number of LEDs the oreo led driver can support -#define OREOLED_BASE_I2C_ADDR 0x68 ///< base i2c address (7-bit) #define OPEOLED_I2C_RETRYCOUNT 2 ///< i2c retry count #define OREOLED_TIMEOUT_USEC 1000000U ///< timeout looking for oreoleds 1 second after startup #define OREOLED_GENERALCALL_US 2000000U ///< general call sent every 2 seconds diff --git a/src/drivers/oreoled/oreoled_bootloader/module.mk b/src/drivers/oreoled/oreoled_bootloader/module.mk new file mode 100644 index 000000000000..f24dc62e3cc1 --- /dev/null +++ b/src/drivers/oreoled/oreoled_bootloader/module.mk @@ -0,0 +1,9 @@ +# +# OreoLED Bootloader Drivers +# + +MODULE_COMMAND = oreoled_bl +SRCS = oreoled_bootloader.cpp \ + oreoled_bootloader_avr.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp new file mode 100644 index 000000000000..4f6ec1fb501f --- /dev/null +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp @@ -0,0 +1,400 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file oreoled.cpp + * @author Angus Peart + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include "oreoled_bootloader.h" +#include "oreoled_bootloader_avr.h" + +/* for now, we only support one OREOLED */ +namespace +{ + OREOLED_BOOTLOADER *g_oreoled_bl = nullptr; +} + +void +OREOLED_BOOTLOADER::cycle_trampoline(void *arg) +{ + OREOLED_BOOTLOADER *dev = (OREOLED_BOOTLOADER *)arg; + + /* check global oreoled_bl and cycle */ + if (g_oreoled_bl != nullptr) { + dev->cycle(); + } +} + +static void oreoled_bl_usage(); +static oreoled_bl_target_t oreoled_bl_parse_target(char* target); + +extern "C" __EXPORT int oreoled_bl_main(int argc, char *argv[]); + +void +oreoled_bl_usage() +{ + warnx("missing command: try 'update [force]', 'kill', 'ping', 'blver', 'ver', 'checksum', 'colour ', 'flash', 'boot'"); + warnx("options:"); + warnx(" -t target (auto)"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + warnx(" -a addr (0x%x)", OREOLED_BASE_I2C_ADDR); +} + +oreoled_bl_target_t +oreoled_bl_parse_target(char *target) +{ + if (!strcmp(target, "avr")) { + return OREOLED_BL_TARGET_AVR; + } else if (!strcmp(target, "auto") == 0) { + return OREOLED_BL_TARGET_DEFAULT; + } + + return OREOLED_BL_TARGET_INVALID; +} + +int +oreoled_bl_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int i2c_addr = OREOLED_BASE_I2C_ADDR; /* 7bit */ + oreoled_bl_target_t target = OREOLED_BL_TARGET_INVALID; + + int ch; + + /* jump over ping/ver/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:t:")) != EOF) { + switch (ch) { + case 'a': + i2c_addr = (int)strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = (int)strtol(optarg, NULL, 0); + break; + + case 't': + target = oreoled_bl_parse_target(strdup(optarg)); + if (target != OREOLED_BL_TARGET_INVALID) { + break; + } + warnx("invalid target '%s' specified", optarg); + + default: + oreoled_bl_usage(); + exit(0); + } + } + + if (optind >= argc) { + oreoled_bl_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int ret; + + /* start driver */ + if (!strcmp(verb, "update")) { + if (g_oreoled_bl != nullptr) { + errx(1, "already started"); + } + + /* by default use LED bus */ + if (i2cdevice == -1) { + i2cdevice = PX4_I2C_BUS_LED; + } + + /* handle update flag */ + bool force_update = false; + if (argc > 2 && !strcmp(argv[2], "force")) { + warnx("forcing update"); + force_update = true; + } + + /* instantiate driver */ + switch (target) { + case OREOLED_BL_TARGET_AVR: + g_oreoled_bl = new OREOLED_BOOTLOADER_AVR(i2cdevice, i2c_addr, force_update); + break; + + default: + errx(1, "unable to instantiate target driver"); + } + + /* check if object was created */ + if (g_oreoled_bl == nullptr) { + errx(1, "failed to allocated memory for driver"); + } + + /* check object was created successfully */ + if (g_oreoled_bl->init() != OK) { + delete g_oreoled_bl; + g_oreoled_bl = nullptr; + errx(1, "failed to start driver"); + } + + /* wait for up to 20 seconds for the driver become ready */ + for (uint8_t i = 0; i < 20; i++) { + if (g_oreoled_bl != nullptr && g_oreoled_bl->is_ready()) { + break; + } + + sleep(1); + } + + exit(0); + } + + /* need the driver past this point */ + if (g_oreoled_bl == nullptr) { + warnx("not started"); + oreoled_bl_usage(); + exit(1); + } + + /* display driver status */ + if (!strcmp(verb, "info")) { + g_oreoled_bl->info(); + exit(0); + } + + if (!strcmp(verb, "kill")) { + /* delete the oreoled object if stop was requested, in addition to turning off the LED. */ + if (!strcmp(verb, "kill")) { + OREOLED_BOOTLOADER *tmp_oreoled = g_oreoled_bl; + g_oreoled_bl = nullptr; + delete tmp_oreoled; + exit(0); + } + + exit(ret); + } + + /* send reset request to all LEDS */ + if (!strcmp(verb, "reset")) { + if (argc < 2) { + errx(1, "Usage: oreoledbl reset"); + } + + int fd = open(OREOLEDBL0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLEDBL0_DEVICE_PATH); + } + + if ((ret = g_oreoled_bl->ioctl(OREOLED_BL_RESET, 0)) != OK) { + errx(1, "failed to run reset"); + } + + close(fd); + exit(ret); + } + + /* attempt to flash all LEDS in bootloader mode*/ + if (!strcmp(verb, "flash")) { + if (argc < 2) { + errx(1, "Usage: oreoledbl flash"); + } + + int fd = open(OREOLEDBL0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLEDBL0_DEVICE_PATH); + } + + if ((ret = g_oreoled_bl->ioctl(OREOLED_BL_FLASH, 0)) != OK) { + errx(1, "failed to run flash"); + } + + close(fd); + exit(ret); + } + + /* send bootloader boot request to all LEDS */ + if (!strcmp(verb, "boot")) { + if (argc < 2) { + errx(1, "Usage: oreoledbl boot"); + } + + int fd = open(OREOLEDBL0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLEDBL0_DEVICE_PATH); + } + + if ((ret = g_oreoled_bl->ioctl(OREOLED_BL_BOOT_APP, 0)) != OK) { + errx(1, "failed to run boot"); + } + + close(fd); + exit(ret); + } + + /* send bootloader ping all LEDs */ + if (!strcmp(verb, "ping")) { + if (argc < 2) { + errx(1, "Usage: oreoledbl ping"); + } + + int fd = open(OREOLEDBL0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLEDBL0_DEVICE_PATH); + } + + if ((ret = g_oreoled_bl->ioctl(OREOLED_BL_PING, 0)) != OK) { + errx(1, "failed to run blping"); + } + + close(fd); + exit(ret); + } + + /* ask all LEDs for their bootloader version */ + if (!strcmp(verb, "blver")) { + if (argc < 2) { + errx(1, "Usage: oreoledbl ver"); + } + + int fd = open(OREOLEDBL0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLEDBL0_DEVICE_PATH); + } + + if ((ret = g_oreoled_bl->ioctl(OREOLED_BL_VER, 0)) != OK) { + errx(1, "failed to get bootloader version"); + } + + close(fd); + exit(ret); + } + + /* ask all LEDs for their application version */ + if (!strcmp(verb, "ver")) { + if (argc < 2) { + errx(1, "Usage: oreoledbl appver"); + } + + int fd = open(OREOLEDBL0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLEDBL0_DEVICE_PATH); + } + + if ((ret = g_oreoled_bl->ioctl(OREOLED_BL_APP_VER, 0)) != OK) { + errx(1, "failed to get app version"); + } + + close(fd); + exit(ret); + } + + /* ask all LEDs for their application checksum */ + if (!strcmp(verb, "checksum")) { + if (argc < 2) { + errx(1, "Usage: oreoledbl checksum"); + } + + int fd = open(OREOLEDBL0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLEDBL0_DEVICE_PATH); + } + + if ((ret = g_oreoled_bl->ioctl(OREOLED_BL_APP_CHECKSUM, 0)) != OK) { + errx(1, "failed to get app checksum"); + } + + close(fd); + exit(ret); + } + + /* set the default bootloader LED colour on all LEDs */ + if (!strcmp(verb, "colour")) { + if (argc < 4) { + errx(1, "Usage: oreoledbl colour "); + } + + int fd = open(OREOLEDBL0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLEDBL0_DEVICE_PATH); + } + + uint8_t red = (uint8_t)strtol(argv[2], NULL, 0); + uint8_t green = (uint8_t)strtol(argv[3], NULL, 0); + oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, 0}; + + if ((ret = g_oreoled_bl->ioctl(OREOLED_BL_SET_COLOUR, (unsigned long)&rgb_set)) != OK) { + errx(1, "failed to set boot startup colours"); + } + + close(fd); + exit(ret); + } + + oreoled_bl_usage(); + exit(0); +} diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h new file mode 100644 index 000000000000..370b4986f34d --- /dev/null +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 oreoled_bootloader.h + * @author Angus Peaty + */ + +#ifndef OREOLED_BOOTLOADER_H +#define OREOLED_BOOTLOADER_H + +#include "oreoled_bootloader.h" + +enum oreoled_bl_target_t { + OREOLED_BL_TARGET_INVALID, + OREOLED_BL_TARGET_AVR, + OREOLED_BL_TARGET_ENUM_COUNT +}; + +#define OREOLED_BL_TARGET_DEFAULT OREOLED_BL_TARGET_AVR + +class OREOLED_BOOTLOADER +{ +public: + OREOLED_BOOTLOADER() {}; + virtual ~OREOLED_BOOTLOADER() {}; + + virtual int init() = 0; + virtual int info() = 0; + virtual int ioctl(unsigned cmd, unsigned long arg) = 0; + + /* Start the update process */ + virtual void start() = 0; + + /* Kill the update process */ + virtual void kill() = 0; + + /* returns true once the driver finished bootloading and ready for commands */ + virtual bool is_ready() = 0; + + void cycle_trampoline(void *arg); + +protected: + virtual void cycle() = 0; +}; + +#endif /* OREOLED_BOOTLOADER_H */ diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp new file mode 100644 index 000000000000..56777745c374 --- /dev/null +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp @@ -0,0 +1,1263 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file oreoled.cpp + * @author Angus Peart + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include "oreoled_bootloader_avr.h" + +#define OREOLED_BL_TARGET_DEFAULT OREOLED_BL_TARGET_AVR + +#define OREOLED_BOOT_FLASH_WAITMS 10 + +#define OREOLED_BOOT_SUPPORTED_VER 0x01 + +#define OREOLED_BOOT_CMD_PING 0x40 +#define OREOLED_BOOT_CMD_BL_VER 0x41 +#define OREOLED_BOOT_CMD_APP_VER 0x42 +#define OREOLED_BOOT_CMD_APP_CHECKSUM 0x43 +#define OREOLED_BOOT_CMD_SET_COLOUR 0x44 + +#define OREOLED_BOOT_CMD_WRITE_FLASH_A 0x50 +#define OREOLED_BOOT_CMD_WRITE_FLASH_B 0x51 +#define OREOLED_BOOT_CMD_FINALISE_FLASH 0x55 + +#define OREOLED_BOOT_CMD_BOOT_APP 0x60 + +#define OREOLED_BL_I2C_RETRYCOUNT 2 +#define OEROLED_BOOT_COMMAND_RETRIES 10 + +/* magic number used to verify the software reset is valid */ +#define OEROLED_RESET_NONCE 0x2A + +#define OREOLED_BOOT_CMD_PING_NONCE 0x2A +#define OREOLED_BOOT_CMD_BOOT_NONCE 0xA2 + +#define OREOLED_FW_FILE_HEADER_LENGTH 2 +#define OREOLED_FW_FILE_SIZE_LIMIT 6144 +#define OREOLED_FW_FILE "/etc/firmware/oreoled.bin" + + +#define OREOLED_STARTUP_TIMEOUT_USEC 2000000U ///< timeout looking for oreoleds 2 seconds after startup +#define OREOLED_STARTUP_INTERVAL_US (1000000U / 10U) ///< time in microseconds, run at 10hz +#define OREOLED_UPDATE_INTERVAL_US (1000000U / 50U) ///< time in microseconds, run at 50hz + +/* constructor */ +OREOLED_BOOTLOADER_AVR::OREOLED_BOOTLOADER_AVR(int bus, int i2c_addr, bool force_update) : + I2C("oreoledbl_avr", OREOLEDBL0_DEVICE_PATH, bus, i2c_addr, 100000), + _work{}, + _num_healthy(0), + _num_inboot(0), + _force_update(force_update), + _is_bootloading(false), + _is_ready(false), + _fw_checksum(0x0000), + _probe_perf(perf_alloc(PC_ELAPSED, "oreoled_bl_probe")), + _comms_errors(perf_alloc(PC_COUNT, "oreoled_bl_comms_errors")), + _reply_errors(perf_alloc(PC_COUNT, "oreoled_bl_reply_errors")) +{ + /* initialise to unhealthy */ + memset(_healthy, 0, sizeof(_healthy)); + + /* initialise to in application */ + memset(_in_boot, 0, sizeof(_in_boot)); + + /* capture startup time */ + _start_time = hrt_absolute_time(); +} + +/* destructor */ +OREOLED_BOOTLOADER_AVR::~OREOLED_BOOTLOADER_AVR() +{ + /* make sure we are truly inactive */ + kill(); + + /* free perf counters */ + perf_free(_probe_perf); + perf_free(_comms_errors); + perf_free(_reply_errors); +} + +int +OREOLED_BOOTLOADER_AVR::init() +{ + int ret; + + /* initialise I2C bus */ + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + /* start work queue */ + start(); + + return OK; +} + +int +OREOLED_BOOTLOADER_AVR::info() +{ + /* print health info on each LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (!_healthy[i]) { + warnx("oreo %u: BAD", (unsigned)i); + + } else { + warnx("oreo %u: OK", (unsigned)i); + } + } + + /* display perf info */ + perf_print_counter(_probe_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_reply_errors); + + return OK; +} + +void +OREOLED_BOOTLOADER_AVR::start() +{ + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED_BOOTLOADER::cycle_trampoline, this, 1); +} + +void +OREOLED_BOOTLOADER_AVR::kill() +{ + work_cancel(HPWORK, &_work); +} + +void +OREOLED_BOOTLOADER_AVR::cycle() +{ + /* check time since startup */ + uint64_t now = hrt_absolute_time(); + bool startup_timeout = (now - _start_time > OREOLED_STARTUP_TIMEOUT_USEC); + + /* during startup period keep searching for unhealthy LEDs */ + if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) { + run_initial_discovery(); + + /* schedule another attempt in 0.1 sec */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED_BOOTLOADER::cycle_trampoline, this, + USEC2TICK(OREOLED_STARTUP_INTERVAL_US)); + return; + } else if (_force_update || _num_inboot || _is_ready) { + run_updates(); + + /* schedule another attempt in 20mS */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED_BOOTLOADER::cycle_trampoline, this, + USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); + return; + } + + kill(); +} + +void +OREOLED_BOOTLOADER_AVR::run_initial_discovery(void) +{ + /* prepare command to turn off LED */ + /* add two bytes of pre-amble to for higher signal to noise ratio */ + uint8_t msg[] = {0xAA, 0x55, OREOLED_PATTERN_OFF, 0x00}; + + /* attempt to contact each unhealthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (!_healthy[i]) { + perf_begin(_probe_perf); + + /* set I2C address */ + set_address(OREOLED_BASE_I2C_ADDR + i); + + /* Calculate XOR checksum and append to the i2c write data */ + msg[sizeof(msg) - 1] = OREOLED_BASE_I2C_ADDR + i; + + for (uint8_t j = 0; j < sizeof(msg) - 1; j++) { + msg[sizeof(msg) - 1] ^= msg[j]; + } + + /* send I2C command */ + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + if (transfer(msg, sizeof(msg), reply, 3) == OK) { + if (reply[1] == OREOLED_BASE_I2C_ADDR + i && + reply[2] == msg[sizeof(msg) - 1]) { + warnx("oreoled %u ok - in bootloader", (unsigned)i); + _healthy[i] = true; + _num_healthy++; + + /* If slaves are in application record that so we can reset if we need to bootload */ + /* This additional check is required for LED firmwares below v1.3 and can be + deprecated once all LEDs in the wild have firmware >= v1.3 */ + if(ping(i) == OK) { + _in_boot[i] = true; + _num_inboot++; + } + + /* Check for a reply with a checksum offset of 1, + which indicates a response from firmwares >= 1.3 */ + } else if (reply[1] == OREOLED_BASE_I2C_ADDR + i && + reply[2] == msg[sizeof(msg) - 1] + 1) { + warnx("oreoled %u ok - in application", (unsigned)i); + _healthy[i] = true; + _num_healthy++; + + } else { + warnx("oreo reply errors: %u", (unsigned)_reply_errors); + perf_count(_reply_errors); + } + + } else { + perf_count(_comms_errors); + } + + perf_end(_probe_perf); + } + } +} + +void +OREOLED_BOOTLOADER_AVR::run_updates(void) +{ + update_application(_force_update); + _force_update = false; + + if (_num_inboot > 0) { + boot_all(); + coerce_healthy(); + _num_inboot = 0; + } else if (!_is_ready) { + /* indicate a ready state since startup has finished */ + _is_ready = true; + } +} + +void +OREOLED_BOOTLOADER_AVR::update_application(bool force_update) +{ + /* check booted oreoleds to see if the app can report it's checksum (release versions >= v1.2) */ + if(!force_update) { + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && !_in_boot[i]) { + /* put any out of date oreoleds into bootloader mode */ + /* being in bootloader mode signals to be code below that the will likey need updating */ + if (inapp_checksum(i) != firmware_checksum()) { + app_reset(i); + } + } + } + } + + /* reset all healthy oreoleds if the number of outdated oreoled's is > 0 */ + /* this is done for consistency, so if only one oreoled is updating, all LEDs show the same behaviour */ + /* otherwise a single oreoled could appear broken or failed. */ + if (_num_inboot > 0 || force_update) { + app_reset_all(); + + /* update each outdated and healthy LED in bootloader mode */ + flash_all(force_update); + + /* boot each healthy LED */ + boot_all(); + + /* coerce LEDs with startup issues to be healthy again */ + coerce_healthy(); + } +} + +int +OREOLED_BOOTLOADER_AVR::app_reset(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + /* send a reset */ + boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + boot_cmd.buff[1] = OREOLED_PARAM_RESET; + boot_cmd.buff[2] = OEROLED_RESET_NONCE; + boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 4; + cmd_add_checksum(&boot_cmd); + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + /* send I2C command with a retry limit */ + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { + if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && + reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + /* slave returned a valid response */ + ret = OK; + /* set this LED as being in boot mode now */ + _in_boot[led_num] = true; + _num_inboot++; + break; + } + } + } + + /* Allow time for the LED to reboot */ + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + + _is_bootloading = false; + return ret; +} + +int +OREOLED_BOOTLOADER_AVR::app_reset_all(void) +{ + int ret = OK; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && !_in_boot[i]) { + /* reset the LED if it's not in the bootloader */ + /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ + if (app_reset(i) != OK) { + ret = -1; + } + } + } + + return ret; +} + +int +OREOLED_BOOTLOADER_AVR::app_ping(int led_num) +{ + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + /* send a pattern off command */ + boot_cmd.buff[0] = 0xAA; + boot_cmd.buff[1] = 0x55; + boot_cmd.buff[2] = OREOLED_PATTERN_OFF; + boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 4; + cmd_add_checksum(&boot_cmd); + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + /* send I2C command with a retry limit */ + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { + if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && + reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + /* slave returned a valid response */ + ret = OK; + break; + } + } + } + + return ret; +} + +uint16_t +OREOLED_BOOTLOADER_AVR::inapp_checksum(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint16_t ret = 0x0000; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + boot_cmd.buff[1] = OREOLED_PARAM_APP_CHECKSUM; + boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 3; + cmd_add_checksum(&boot_cmd); + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_PARAM_APP_CHECKSUM && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl app checksum OK from LED %i", boot_cmd.led_num); + warnx("bl app checksum msb: 0x%x", reply[3]); + warnx("bl app checksum lsb: 0x%x", reply[4]); + ret = ((reply[3] << 8) | reply[4]); + break; + + } else { + warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); + warnx("bl app checksum response ADDR: 0x%x", reply[1]); + warnx("bl app checksum response CMD: 0x%x", reply[2]); + warnx("bl app checksum response VER H: 0x%x", reply[3]); + warnx("bl app checksum response VER L: 0x%x", reply[4]); + warnx("bl app checksum response XOR: 0x%x", reply[5]); + + if (retry > 1) { + warnx("bl app checksum retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl app checksum failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +int +OREOLED_BOOTLOADER_AVR::ping(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_PING; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + cmd_add_checksum(&boot_cmd); + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_PING && + reply[3] == OREOLED_BOOT_CMD_PING_NONCE && + reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl ping OK from LED %i", boot_cmd.led_num); + ret = OK; + break; + + } else { + warnx("bl ping FAIL from LED %i", boot_cmd.led_num); + warnx("bl ping response ADDR: 0x%x", reply[1]); + warnx("bl ping response CMD: 0x%x", reply[2]); + warnx("bl ping response NONCE: 0x%x", reply[3]); + warnx("bl ping response XOR: 0x%x", reply[4]); + + if (retry > 1) { + warnx("bl ping retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl ping failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +uint8_t +OREOLED_BOOTLOADER_AVR::version(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint8_t ret = 0x00; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_BL_VER; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + cmd_add_checksum(&boot_cmd); + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BL_VER && + reply[3] == OREOLED_BOOT_SUPPORTED_VER && + reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl ver from LED %i = %i", boot_cmd.led_num, reply[3]); + ret = reply[3]; + break; + + } else { + warnx("bl ver response ADDR: 0x%x", reply[1]); + warnx("bl ver response CMD: 0x%x", reply[2]); + warnx("bl ver response VER: 0x%x", reply[3]); + warnx("bl ver response XOR: 0x%x", reply[4]); + + if (retry > 1) { + warnx("bl ver retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl ver failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +uint16_t +OREOLED_BOOTLOADER_AVR::app_version(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint16_t ret = 0x0000; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_VER; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + cmd_add_checksum(&boot_cmd); + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_APP_VER && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl app version OK from LED %i", boot_cmd.led_num); + warnx("bl app version msb: 0x%x", reply[3]); + warnx("bl app version lsb: 0x%x", reply[4]); + ret = ((reply[3] << 8) | reply[4]); + break; + + } else { + warnx("bl app version FAIL from LED %i", boot_cmd.led_num); + warnx("bl app version response ADDR: 0x%x", reply[1]); + warnx("bl app version response CMD: 0x%x", reply[2]); + warnx("bl app version response VER H: 0x%x", reply[3]); + warnx("bl app version response VER L: 0x%x", reply[4]); + warnx("bl app version response XOR: 0x%x", reply[5]); + + if (retry > 1) { + warnx("bl app version retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl app version failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +uint16_t +OREOLED_BOOTLOADER_AVR::app_checksum(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + uint16_t ret = 0x0000; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_CHECKSUM; + boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 2; + cmd_add_checksum(&boot_cmd); + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_APP_CHECKSUM && + reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl app checksum OK from LED %i", boot_cmd.led_num); + warnx("bl app checksum msb: 0x%x", reply[3]); + warnx("bl app checksum lsb: 0x%x", reply[4]); + ret = ((reply[3] << 8) | reply[4]); + break; + + } else { + warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); + warnx("bl app checksum response ADDR: 0x%x", reply[1]); + warnx("bl app checksum response CMD: 0x%x", reply[2]); + warnx("bl app checksum response VER H: 0x%x", reply[3]); + warnx("bl app checksum response VER L: 0x%x", reply[4]); + warnx("bl app checksum response XOR: 0x%x", reply[5]); + + if (retry > 1) { + warnx("bl app checksum retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl app checksum failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +int +OREOLED_BOOTLOADER_AVR::set_colour(int led_num, uint8_t red, uint8_t green) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_SET_COLOUR; + boot_cmd.buff[1] = red; + boot_cmd.buff[2] = green; + boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 4; + cmd_add_checksum(&boot_cmd); + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_SET_COLOUR && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl set colour OK from LED %i", boot_cmd.led_num); + ret = OK; + break; + + } else { + warnx("bl set colour FAIL from LED %i", boot_cmd.led_num); + warnx("bl set colour response ADDR: 0x%x", reply[1]); + warnx("bl set colour response CMD: 0x%x", reply[2]); + warnx("bl set colour response XOR: 0x%x", reply[3]); + + if (retry > 1) { + warnx("bl app colour retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl app colour failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + _is_bootloading = false; + return ret; +} + +int +OREOLED_BOOTLOADER_AVR::flash(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + /* Open the bootloader file */ + int fd = ::open(OREOLED_FW_FILE, O_RDONLY); + + /* check for error opening the file */ + if (fd < 0) { + return -1; + } + + struct stat s; + + /* attempt to stat the file */ + if (stat(OREOLED_FW_FILE, &s) != 0) { + ::close(fd); + return -1; + } + + uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; + + /* sanity-check file size */ + if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { + ::close(fd); + return -1; + } + + uint8_t *buf = new uint8_t[s.st_size]; + + /* check that the buffer has been allocated */ + if (buf == NULL) { + ::close(fd); + return -1; + } + + /* check that the firmware can be read into the buffer */ + if (::read(fd, buf, s.st_size) != s.st_size) { + ::close(fd); + delete[] buf; + return -1; + } + + ::close(fd); + + /* Grab the version bytes from the binary */ + uint8_t version_major = buf[0]; + uint8_t version_minor = buf[1]; + + /* calculate flash pages (rounded up to nearest integer) */ + uint8_t flash_pages = ((fw_length + 64 - 1) / 64); + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + + /* Loop through flash pages */ + for (uint8_t page_idx = 0; page_idx < flash_pages; page_idx++) { + + /* Send the first half of the 64 byte flash page */ + memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); + boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_A; + boot_cmd.buff[1] = page_idx; + memcpy(boot_cmd.buff + 2, buf + (page_idx * 64) + OREOLED_FW_FILE_HEADER_LENGTH, 32); + boot_cmd.buff[32 + 2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 32 + 3; + cmd_add_checksum(&boot_cmd); + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_A && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl flash %ia OK for LED %i", page_idx, boot_cmd.led_num); + break; + + } else { + warnx("bl flash %ia FAIL for LED %i", page_idx, boot_cmd.led_num); + warnx("bl flash %ia response ADDR: 0x%x", page_idx, reply[1]); + warnx("bl flash %ia response CMD: 0x%x", page_idx, reply[2]); + warnx("bl flash %ia response XOR: 0x%x", page_idx, reply[3]); + + if (retry > 1) { + warnx("bl flash %ia retrying LED %i", page_idx, boot_cmd.led_num); + + } else { + warnx("bl flash %ia failed on LED %i", page_idx, boot_cmd.led_num); + delete[] buf; + return -1; + } + } + } + + /* Send the second half of the 64 byte flash page */ + memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); + boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_B; + memcpy(boot_cmd.buff + 1, buf + (page_idx * 64) + 32 + OREOLED_FW_FILE_HEADER_LENGTH, 32); + boot_cmd.buff[32 + 1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 32 + 2; + cmd_add_checksum(&boot_cmd); + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write+Read */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_B && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl flash %ib OK for LED %i", page_idx, boot_cmd.led_num); + break; + + } else { + warnx("bl flash %ib FAIL for LED %i", page_idx, boot_cmd.led_num); + warnx("bl flash %ib response ADDR: 0x%x", page_idx, reply[1]); + warnx("bl flash %ib response CMD: 0x%x", page_idx, reply[2]); + warnx("bl flash %ib response XOR: 0x%x", page_idx, reply[3]); + + if (retry > 1) { + warnx("bl flash %ib retrying LED %i", page_idx, boot_cmd.led_num); + + } else { + errx(1, "bl flash %ib failed on LED %i", page_idx, boot_cmd.led_num); + delete[] buf; + return -1; + } + } + } + + /* Sleep to allow flash to write */ + /* Wait extra long on the first write, to allow time for EEPROM updates */ + if (page_idx == 0) { + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + + } else { + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000); + } + } + + uint16_t calculated_app_checksum = firmware_checksum(); + + /* Flash writes must have succeeded so finalise the flash */ + boot_cmd.buff[0] = OREOLED_BOOT_CMD_FINALISE_FLASH; + boot_cmd.buff[1] = version_major; + boot_cmd.buff[2] = version_minor; + boot_cmd.buff[3] = (uint8_t)(fw_length >> 8); + boot_cmd.buff[4] = (uint8_t)(fw_length & 0xFF); + boot_cmd.buff[5] = (uint8_t)(calculated_app_checksum >> 8); + boot_cmd.buff[6] = (uint8_t)(calculated_app_checksum & 0xFF); + boot_cmd.buff[7] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 8; + cmd_add_checksum(&boot_cmd); + + /* Try to finalise for twice the amount of normal retries */ + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES * 2; retry > 0; retry--) { + /* Send the I2C Write */ + memset(reply, 0, sizeof(reply)); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_FINALISE_FLASH && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl finalise OK from LED %i", boot_cmd.led_num); + break; + + } else { + warnx("bl finalise response ADDR: 0x%x", reply[1]); + warnx("bl finalise response CMD: 0x%x", reply[2]); + warnx("bl finalise response XOR: 0x%x", reply[3]); + + if (retry > 1) { + warnx("bl finalise retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl finalise failed on LED %i", boot_cmd.led_num); + delete[] buf; + return -1; + } + } + } + + /* allow time for flash to finalise */ + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + + /* clean up file buffer */ + delete[] buf; + + _is_bootloading = false; + return 1; +} + +int +OREOLED_BOOTLOADER_AVR::boot(int led_num) +{ + _is_bootloading = true; + oreoled_cmd_t boot_cmd; + boot_cmd.led_num = led_num; + + int ret = -1; + + /* Set the current address */ + set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + + boot_cmd.buff[0] = OREOLED_BOOT_CMD_BOOT_APP; + boot_cmd.buff[1] = OREOLED_BOOT_CMD_BOOT_NONCE; + boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; + boot_cmd.num_bytes = 3; + cmd_add_checksum(&boot_cmd); + + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { + /* Send the I2C Write */ + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + + /* Check the response */ + if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BOOT_APP && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl boot OK from LED %i", boot_cmd.led_num); + /* decrement the inboot counter so we don't get confused */ + _in_boot[led_num] = false; + _num_inboot--; + ret = OK; + break; + + } else if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && + reply[2] == OREOLED_BOOT_CMD_BOOT_NONCE && + reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + warnx("bl boot error from LED %i: no app", boot_cmd.led_num); + break; + + } else { + warnx("bl boot response ADDR: 0x%x", reply[1]); + warnx("bl boot response CMD: 0x%x", reply[2]); + warnx("bl boot response XOR: 0x%x", reply[3]); + + if (retry > 1) { + warnx("bl boot retrying LED %i", boot_cmd.led_num); + + } else { + warnx("bl boot failed on LED %i", boot_cmd.led_num); + break; + } + } + } + + /* allow time for the LEDs to boot */ + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); + + _is_bootloading = false; + return ret; +} + +int +OREOLED_BOOTLOADER_AVR::boot_all(void) { + int ret = OK; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && _in_boot[i]) { + /* boot the application */ + if (boot(i) != OK) { + ret = -1; + } + } + } + + return ret; +} + +uint16_t +OREOLED_BOOTLOADER_AVR::firmware_checksum(void) +{ + /* Calculate the 16 bit XOR checksum of the firmware on the first call of this function */ + if (_fw_checksum == 0x0000) { + /* Open the bootloader file */ + int fd = ::open(OREOLED_FW_FILE, O_RDONLY); + + /* check for error opening the file */ + if (fd < 0) { + return -1; + } + + struct stat s; + + /* attempt to stat the file */ + if (stat(OREOLED_FW_FILE, &s) != 0) { + ::close(fd); + return -1; + } + + uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; + + /* sanity-check file size */ + if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { + ::close(fd); + return -1; + } + + uint8_t *buf = new uint8_t[s.st_size]; + + /* check that the buffer has been allocated */ + if (buf == NULL) { + ::close(fd); + return -1; + } + + /* check that the firmware can be read into the buffer */ + if (::read(fd, buf, s.st_size) != s.st_size) { + ::close(fd); + delete[] buf; + return -1; + } + + ::close(fd); + + /* Calculate a 16 bit XOR checksum of the flash */ + /* Skip the first two bytes which are the version information, plus + the next two bytes which are modified by the bootloader */ + uint16_t calculated_app_checksum = 0x0000; + + for (uint16_t j = 2 + OREOLED_FW_FILE_HEADER_LENGTH; j < s.st_size; j += 2) { + calculated_app_checksum ^= (buf[j] << 8) | buf[j + 1]; + } + + delete[] buf; + + warnx("fw length = %i", fw_length); + warnx("fw checksum = %i", calculated_app_checksum); + + /* Store the checksum so it's only calculated once */ + _fw_checksum = calculated_app_checksum; + } + + return _fw_checksum; +} + +int +OREOLED_BOOTLOADER_AVR::flash_all(bool force_update) +{ + int ret = -1; + + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i] && _in_boot[i]) { + int result; + + if (force_update) { + result = flash(i); + } else if (app_checksum(i) != firmware_checksum()) { + /* only flash LEDs with an old version of the applictioon */ + result = flash(i); + } + + if (result != OK) { + ret = -1; + } + } + } + + return ret; +} + +int +OREOLED_BOOTLOADER_AVR::coerce_healthy(void) +{ + int ret = -1; + + /* check each unhealthy LED */ + /* this re-checks "unhealthy" LEDs as they can sometimes power up with the wrong ID, */ + /* but will have the correct ID once they jump to the application and be healthy again */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (!_healthy[i] && app_ping(i) == OK) { + /* mark as healthy */ + _healthy[i] = true; + _num_healthy++; + ret = OK; + } + } + + return ret; +} + +void +OREOLED_BOOTLOADER_AVR::cmd_add_checksum(oreoled_cmd_t* cmd) +{ + if (cmd->num_bytes == 0 || cmd->num_bytes >= OREOLED_CMD_LENGTH_MAX) { + return; + } + + /* write a basic 8-bit XOR checksum into the last byte of the command bytes array*/ + uint8_t checksum_idx = cmd->num_bytes - 1; + for (uint8_t i = 0; i < checksum_idx; i++) { + cmd->buff[checksum_idx] ^= cmd->buff[i]; + } +} + +int +OREOLED_BOOTLOADER_AVR::ioctl(unsigned cmd, unsigned long arg) +{ + int ret = -ENODEV; + + switch (cmd) { + case OREOLED_BL_RESET: + ret = app_reset_all(); + return ret; + + case OREOLED_BL_PING: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + ping(i); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_VER: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + version(i); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_FLASH: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + flash(i); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_APP_VER: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + app_version(i); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_APP_CHECKSUM: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + app_checksum(i); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_SET_COLOUR: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + set_colour(i, ((oreoled_rgbset_t *) arg)->red, ((oreoled_rgbset_t *) arg)->green); + ret = OK; + } + } + + return ret; + + case OREOLED_BL_BOOT_APP: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + boot(i); + ret = OK; + } + } + + return ret; + + default: + ret = EINVAL; + } + + return ret; +} + +/* return the internal _is_ready flag indicating if initialisation is complete */ +bool +OREOLED_BOOTLOADER_AVR::is_ready() +{ + return _is_ready; +} diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h new file mode 100644 index 000000000000..2970e7348935 --- /dev/null +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 oreoled_bootloader_avr.h + * @author Angus Peaty + */ + +#ifndef OREOLED_BOOTLOADER_AVR_H +#define OREOLED_BOOTLOADER_AVR_H + +#include "oreoled_bootloader.h" + +class OREOLED_BOOTLOADER_AVR : public OREOLED_BOOTLOADER, public device::I2C +{ +public: + OREOLED_BOOTLOADER_AVR(int bus, int i2c_addr, bool force_update); + ~OREOLED_BOOTLOADER_AVR(); + + int init(); + int info(); + int ioctl(unsigned cmd, unsigned long arg); + + void start(); + void kill(); + + /* returns true once the driver finished bootloading and ready for commands */ + bool is_ready(); + +protected: + void cycle(); + +private: + void run_initial_discovery(void); + void run_updates(void); + + void update_application(bool force_update); + int app_reset(int led_num); + int app_reset_all(void); + int app_ping(int led_num); + uint16_t inapp_checksum(int led_num); + int ping(int led_num); + uint8_t version(int led_num); + uint16_t app_version(int led_num); + uint16_t app_checksum(int led_num); + int set_colour(int led_num, uint8_t red, uint8_t green); + int flash(int led_num); + int flash_all(bool force_update); + int boot(int led_num); + int boot_all(void); + uint16_t firmware_checksum(void); + int coerce_healthy(void); + void cmd_add_checksum(oreoled_cmd_t* cmd); + + /* internal variables */ + work_s _work; ///< work queue for scheduling reads + bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED + bool _in_boot[OREOLED_NUM_LEDS]; ///< true for each LED that is in bootloader mode + uint8_t _num_healthy; ///< number of healthy LEDs + uint8_t _num_inboot; ///< number of LEDs in bootloader + uint64_t _start_time; ///< system time we first attempt to communicate with battery + bool _force_update; ///< true if the driver should update all LEDs + bool _is_bootloading; ///< true if a bootloading operation is in progress + bool _is_ready; ///< set to true once the driver has completly initialised + uint16_t _fw_checksum; ///< the current 16bit XOR checksum of the built in oreoled firmware binary + + /* performance checking */ + perf_counter_t _probe_perf; + perf_counter_t _comms_errors; + perf_counter_t _reply_errors; +}; + +#endif /* OREOLED_BOOTLOADER_AVR_H */ From 5052d0ddbb0e365bd44beabb5df59641d48c31ae Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Thu, 19 Nov 2015 03:09:34 +1100 Subject: [PATCH 284/293] oreoledbl: clean up cycle task --- .../oreoled_bootloader/oreoled_bootloader_avr.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp index 56777745c374..c86209dd6018 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp @@ -206,15 +206,10 @@ OREOLED_BOOTLOADER_AVR::cycle() work_queue(HPWORK, &_work, (worker_t)&OREOLED_BOOTLOADER::cycle_trampoline, this, USEC2TICK(OREOLED_STARTUP_INTERVAL_US)); return; - } else if (_force_update || _num_inboot || _is_ready) { - run_updates(); - - /* schedule another attempt in 20mS */ - work_queue(HPWORK, &_work, (worker_t)&OREOLED_BOOTLOADER::cycle_trampoline, this, - USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); - return; } + run_updates(); + kill(); } @@ -289,7 +284,9 @@ OREOLED_BOOTLOADER_AVR::run_updates(void) boot_all(); coerce_healthy(); _num_inboot = 0; - } else if (!_is_ready) { + } + + if (!_is_ready) { /* indicate a ready state since startup has finished */ _is_ready = true; } From 5d3ae541669a29730a205e1df9f6c08262a00753 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Thu, 19 Nov 2015 12:20:46 +1100 Subject: [PATCH 285/293] oreoledbl: simplify interface and remove work queue usage --- .../oreoled_bootloader/oreoled_bootloader.cpp | 28 +- .../oreoled_bootloader/oreoled_bootloader.h | 19 +- .../oreoled_bootloader_avr.cpp | 241 +++++++----------- .../oreoled_bootloader_avr.h | 43 ++-- 4 files changed, 115 insertions(+), 216 deletions(-) diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp index 4f6ec1fb501f..b5ae6f2bd7fe 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp @@ -74,17 +74,6 @@ namespace OREOLED_BOOTLOADER *g_oreoled_bl = nullptr; } -void -OREOLED_BOOTLOADER::cycle_trampoline(void *arg) -{ - OREOLED_BOOTLOADER *dev = (OREOLED_BOOTLOADER *)arg; - - /* check global oreoled_bl and cycle */ - if (g_oreoled_bl != nullptr) { - dev->cycle(); - } -} - static void oreoled_bl_usage(); static oreoled_bl_target_t oreoled_bl_parse_target(char* target); @@ -188,21 +177,12 @@ oreoled_bl_main(int argc, char *argv[]) } /* check object was created successfully */ - if (g_oreoled_bl->init() != OK) { + if (g_oreoled_bl->update() != OK) { delete g_oreoled_bl; g_oreoled_bl = nullptr; errx(1, "failed to start driver"); } - /* wait for up to 20 seconds for the driver become ready */ - for (uint8_t i = 0; i < 20; i++) { - if (g_oreoled_bl != nullptr && g_oreoled_bl->is_ready()) { - break; - } - - sleep(1); - } - exit(0); } @@ -213,12 +193,6 @@ oreoled_bl_main(int argc, char *argv[]) exit(1); } - /* display driver status */ - if (!strcmp(verb, "info")) { - g_oreoled_bl->info(); - exit(0); - } - if (!strcmp(verb, "kill")) { /* delete the oreoled object if stop was requested, in addition to turning off the LED. */ if (!strcmp(verb, "kill")) { diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h index 370b4986f34d..39e773dabeed 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h @@ -55,23 +55,8 @@ class OREOLED_BOOTLOADER OREOLED_BOOTLOADER() {}; virtual ~OREOLED_BOOTLOADER() {}; - virtual int init() = 0; - virtual int info() = 0; - virtual int ioctl(unsigned cmd, unsigned long arg) = 0; - - /* Start the update process */ - virtual void start() = 0; - - /* Kill the update process */ - virtual void kill() = 0; - - /* returns true once the driver finished bootloading and ready for commands */ - virtual bool is_ready() = 0; - - void cycle_trampoline(void *arg); - -protected: - virtual void cycle() = 0; + virtual int update(void) = 0; + virtual int ioctl(const unsigned cmd, const unsigned long arg) = 0; }; #endif /* OREOLED_BOOTLOADER_H */ diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp index c86209dd6018..e2b88a4c52c9 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp @@ -111,7 +111,6 @@ OREOLED_BOOTLOADER_AVR::OREOLED_BOOTLOADER_AVR(int bus, int i2c_addr, bool force _num_inboot(0), _force_update(force_update), _is_bootloading(false), - _is_ready(false), _fw_checksum(0x0000), _probe_perf(perf_alloc(PC_ELAPSED, "oreoled_bl_probe")), _comms_errors(perf_alloc(PC_COUNT, "oreoled_bl_comms_errors")), @@ -130,9 +129,6 @@ OREOLED_BOOTLOADER_AVR::OREOLED_BOOTLOADER_AVR(int bus, int i2c_addr, bool force /* destructor */ OREOLED_BOOTLOADER_AVR::~OREOLED_BOOTLOADER_AVR() { - /* make sure we are truly inactive */ - kill(); - /* free perf counters */ perf_free(_probe_perf); perf_free(_comms_errors); @@ -140,7 +136,7 @@ OREOLED_BOOTLOADER_AVR::~OREOLED_BOOTLOADER_AVR() } int -OREOLED_BOOTLOADER_AVR::init() +OREOLED_BOOTLOADER_AVR::update(void) { int ret; @@ -151,14 +147,83 @@ OREOLED_BOOTLOADER_AVR::init() return ret; } - /* start work queue */ - start(); + startup_discovery(); + run_updates(); + + print_info(); return OK; } int -OREOLED_BOOTLOADER_AVR::info() +OREOLED_BOOTLOADER_AVR::ioctl(const unsigned cmd, const unsigned long arg) +{ + int ret = EINVAL; + + switch (cmd) { + case OREOLED_BL_RESET: + ret = app_reset_all(); + return ret; + + case OREOLED_BL_PING: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + ping(i); + ret = OK; + } + } + return ret; + + case OREOLED_BL_VER: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + version(i); + ret = OK; + } + } + return ret; + + case OREOLED_BL_FLASH: + ret = flash_all(true); + return ret; + + case OREOLED_BL_APP_VER: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + app_version(i); + ret = OK; + } + } + return ret; + + case OREOLED_BL_APP_CHECKSUM: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + app_checksum(i); + ret = OK; + } + } + return ret; + + case OREOLED_BL_SET_COLOUR: + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (_healthy[i]) { + set_colour(i, ((oreoled_rgbset_t *) arg)->red, ((oreoled_rgbset_t *) arg)->green); + ret = OK; + } + } + return ret; + + case OREOLED_BL_BOOT_APP: + ret = boot_all(); + return ret; + } + + return ret; +} + +void +OREOLED_BOOTLOADER_AVR::print_info(void) { /* print health info on each LED */ for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { @@ -174,47 +239,30 @@ OREOLED_BOOTLOADER_AVR::info() perf_print_counter(_probe_perf); perf_print_counter(_comms_errors); perf_print_counter(_reply_errors); - - return OK; } void -OREOLED_BOOTLOADER_AVR::start() -{ - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&OREOLED_BOOTLOADER::cycle_trampoline, this, 1); -} - -void -OREOLED_BOOTLOADER_AVR::kill() -{ - work_cancel(HPWORK, &_work); -} - -void -OREOLED_BOOTLOADER_AVR::cycle() +OREOLED_BOOTLOADER_AVR::startup_discovery(void) { /* check time since startup */ uint64_t now = hrt_absolute_time(); bool startup_timeout = (now - _start_time > OREOLED_STARTUP_TIMEOUT_USEC); /* during startup period keep searching for unhealthy LEDs */ - if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) { - run_initial_discovery(); - - /* schedule another attempt in 0.1 sec */ - work_queue(HPWORK, &_work, (worker_t)&OREOLED_BOOTLOADER::cycle_trampoline, this, - USEC2TICK(OREOLED_STARTUP_INTERVAL_US)); - return; - } + do { + discover(); - run_updates(); + if (_num_healthy == OREOLED_NUM_LEDS) { + break; + } - kill(); + /* wait for 0.1 sec before running discovery again */ + usleep(OREOLED_STARTUP_INTERVAL_US); + } while ((hrt_absolute_time() - _start_time) < OREOLED_STARTUP_TIMEOUT_USEC); } void -OREOLED_BOOTLOADER_AVR::run_initial_discovery(void) +OREOLED_BOOTLOADER_AVR::discover(void) { /* prepare command to turn off LED */ /* add two bytes of pre-amble to for higher signal to noise ratio */ @@ -285,15 +333,10 @@ OREOLED_BOOTLOADER_AVR::run_updates(void) coerce_healthy(); _num_inboot = 0; } - - if (!_is_ready) { - /* indicate a ready state since startup has finished */ - _is_ready = true; - } } void -OREOLED_BOOTLOADER_AVR::update_application(bool force_update) +OREOLED_BOOTLOADER_AVR::update_application(const bool force_update) { /* check booted oreoleds to see if the app can report it's checksum (release versions >= v1.2) */ if(!force_update) { @@ -326,7 +369,7 @@ OREOLED_BOOTLOADER_AVR::update_application(bool force_update) } int -OREOLED_BOOTLOADER_AVR::app_reset(int led_num) +OREOLED_BOOTLOADER_AVR::app_reset(const int led_num) { _is_bootloading = true; oreoled_cmd_t boot_cmd; @@ -391,7 +434,7 @@ OREOLED_BOOTLOADER_AVR::app_reset_all(void) } int -OREOLED_BOOTLOADER_AVR::app_ping(int led_num) +OREOLED_BOOTLOADER_AVR::app_ping(const int led_num) { oreoled_cmd_t boot_cmd; boot_cmd.led_num = led_num; @@ -427,7 +470,7 @@ OREOLED_BOOTLOADER_AVR::app_ping(int led_num) } uint16_t -OREOLED_BOOTLOADER_AVR::inapp_checksum(int led_num) +OREOLED_BOOTLOADER_AVR::inapp_checksum(const int led_num) { _is_bootloading = true; oreoled_cmd_t boot_cmd; @@ -484,7 +527,7 @@ OREOLED_BOOTLOADER_AVR::inapp_checksum(int led_num) } int -OREOLED_BOOTLOADER_AVR::ping(int led_num) +OREOLED_BOOTLOADER_AVR::ping(const int led_num) { _is_bootloading = true; oreoled_cmd_t boot_cmd; @@ -538,7 +581,7 @@ OREOLED_BOOTLOADER_AVR::ping(int led_num) } uint8_t -OREOLED_BOOTLOADER_AVR::version(int led_num) +OREOLED_BOOTLOADER_AVR::version(const int led_num) { _is_bootloading = true; oreoled_cmd_t boot_cmd; @@ -591,7 +634,7 @@ OREOLED_BOOTLOADER_AVR::version(int led_num) } uint16_t -OREOLED_BOOTLOADER_AVR::app_version(int led_num) +OREOLED_BOOTLOADER_AVR::app_version(const int led_num) { _is_bootloading = true; oreoled_cmd_t boot_cmd; @@ -647,7 +690,7 @@ OREOLED_BOOTLOADER_AVR::app_version(int led_num) } uint16_t -OREOLED_BOOTLOADER_AVR::app_checksum(int led_num) +OREOLED_BOOTLOADER_AVR::app_checksum(const int led_num) { _is_bootloading = true; oreoled_cmd_t boot_cmd; @@ -703,7 +746,7 @@ OREOLED_BOOTLOADER_AVR::app_checksum(int led_num) } int -OREOLED_BOOTLOADER_AVR::set_colour(int led_num, uint8_t red, uint8_t green) +OREOLED_BOOTLOADER_AVR::set_colour(const int led_num, const uint8_t red, const uint8_t green) { _is_bootloading = true; oreoled_cmd_t boot_cmd; @@ -757,7 +800,7 @@ OREOLED_BOOTLOADER_AVR::set_colour(int led_num, uint8_t red, uint8_t green) } int -OREOLED_BOOTLOADER_AVR::flash(int led_num) +OREOLED_BOOTLOADER_AVR::flash(const int led_num) { _is_bootloading = true; oreoled_cmd_t boot_cmd; @@ -959,7 +1002,7 @@ OREOLED_BOOTLOADER_AVR::flash(int led_num) } int -OREOLED_BOOTLOADER_AVR::boot(int led_num) +OREOLED_BOOTLOADER_AVR::boot(const int led_num) { _is_bootloading = true; oreoled_cmd_t boot_cmd; @@ -1107,7 +1150,7 @@ OREOLED_BOOTLOADER_AVR::firmware_checksum(void) } int -OREOLED_BOOTLOADER_AVR::flash_all(bool force_update) +OREOLED_BOOTLOADER_AVR::flash_all(const bool force_update) { int ret = -1; @@ -1164,97 +1207,3 @@ OREOLED_BOOTLOADER_AVR::cmd_add_checksum(oreoled_cmd_t* cmd) cmd->buff[checksum_idx] ^= cmd->buff[i]; } } - -int -OREOLED_BOOTLOADER_AVR::ioctl(unsigned cmd, unsigned long arg) -{ - int ret = -ENODEV; - - switch (cmd) { - case OREOLED_BL_RESET: - ret = app_reset_all(); - return ret; - - case OREOLED_BL_PING: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - ping(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_VER: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - version(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_FLASH: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - flash(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_APP_VER: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - app_version(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_APP_CHECKSUM: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - app_checksum(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_SET_COLOUR: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - set_colour(i, ((oreoled_rgbset_t *) arg)->red, ((oreoled_rgbset_t *) arg)->green); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_BOOT_APP: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - boot(i); - ret = OK; - } - } - - return ret; - - default: - ret = EINVAL; - } - - return ret; -} - -/* return the internal _is_ready flag indicating if initialisation is complete */ -bool -OREOLED_BOOTLOADER_AVR::is_ready() -{ - return _is_ready; -} diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h index 2970e7348935..9d44b3c4bb96 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h @@ -47,36 +47,28 @@ class OREOLED_BOOTLOADER_AVR : public OREOLED_BOOTLOADER, public device::I2C OREOLED_BOOTLOADER_AVR(int bus, int i2c_addr, bool force_update); ~OREOLED_BOOTLOADER_AVR(); - int init(); - int info(); - int ioctl(unsigned cmd, unsigned long arg); - - void start(); - void kill(); - - /* returns true once the driver finished bootloading and ready for commands */ - bool is_ready(); - -protected: - void cycle(); + int update(void); + int ioctl(const unsigned cmd, const unsigned long arg); private: - void run_initial_discovery(void); + void print_info(void); + void startup_discovery(void); + void discover(void); void run_updates(void); - void update_application(bool force_update); - int app_reset(int led_num); + void update_application(const bool force_update); + int app_reset(const int led_num); int app_reset_all(void); - int app_ping(int led_num); - uint16_t inapp_checksum(int led_num); - int ping(int led_num); - uint8_t version(int led_num); - uint16_t app_version(int led_num); - uint16_t app_checksum(int led_num); - int set_colour(int led_num, uint8_t red, uint8_t green); - int flash(int led_num); - int flash_all(bool force_update); - int boot(int led_num); + int app_ping(const int led_num); + uint16_t inapp_checksum(const int led_num); + int ping(const int led_num); + uint8_t version(const int led_num); + uint16_t app_version(const int led_num); + uint16_t app_checksum(const int led_num); + int set_colour(const int led_num, const uint8_t red, const uint8_t green); + int flash(const int led_num); + int flash_all(const bool force_update); + int boot(const int led_num); int boot_all(void); uint16_t firmware_checksum(void); int coerce_healthy(void); @@ -91,7 +83,6 @@ class OREOLED_BOOTLOADER_AVR : public OREOLED_BOOTLOADER, public device::I2C uint64_t _start_time; ///< system time we first attempt to communicate with battery bool _force_update; ///< true if the driver should update all LEDs bool _is_bootloading; ///< true if a bootloading operation is in progress - bool _is_ready; ///< set to true once the driver has completly initialised uint16_t _fw_checksum; ///< the current 16bit XOR checksum of the built in oreoled firmware binary /* performance checking */ From 58b07cefb663edd959bac3730d97a33e77159aca Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Mon, 23 Nov 2015 15:56:15 +1100 Subject: [PATCH 286/293] oreoledbl: minor cleanup and bug fixes --- .../oreoled/oreoled_bootloader/module.mk | 2 +- .../oreoled_bootloader/oreoled_bootloader.cpp | 70 +++--- .../oreoled_bootloader/oreoled_bootloader.h | 3 +- .../oreoled_bootloader_avr.cpp | 221 +++++++++--------- .../oreoled_bootloader_avr.h | 13 +- 5 files changed, 161 insertions(+), 148 deletions(-) diff --git a/src/drivers/oreoled/oreoled_bootloader/module.mk b/src/drivers/oreoled/oreoled_bootloader/module.mk index f24dc62e3cc1..e53864f11e61 100644 --- a/src/drivers/oreoled/oreoled_bootloader/module.mk +++ b/src/drivers/oreoled/oreoled_bootloader/module.mk @@ -2,7 +2,7 @@ # OreoLED Bootloader Drivers # -MODULE_COMMAND = oreoled_bl +MODULE_COMMAND = oreoledbl SRCS = oreoled_bootloader.cpp \ oreoled_bootloader_avr.cpp diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp index b5ae6f2bd7fe..4f0cf0438e8f 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp @@ -74,27 +74,34 @@ namespace OREOLED_BOOTLOADER *g_oreoled_bl = nullptr; } -static void oreoled_bl_usage(); +static void oreoled_bl_init_usage(); +static void oreoled_bl_cmd_usage(); static oreoled_bl_target_t oreoled_bl_parse_target(char* target); -extern "C" __EXPORT int oreoled_bl_main(int argc, char *argv[]); +extern "C" __EXPORT int oreoledbl_main(int argc, char *argv[]); void -oreoled_bl_usage() +oreoled_bl_init_usage() { - warnx("missing command: try 'update [force]', 'kill', 'ping', 'blver', 'ver', 'checksum', 'colour ', 'flash', 'boot'"); + warnx("missing command: try 'start', 'update [force]'"); warnx("options:"); warnx(" -t target (auto)"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", OREOLED_BASE_I2C_ADDR); } +void +oreoled_bl_cmd_usage() +{ + warnx("missing command: try 'stop', 'ping', 'ver', 'appver', 'checksum', 'colour ', 'flash', 'boot'"); +} + oreoled_bl_target_t oreoled_bl_parse_target(char *target) { if (!strcmp(target, "avr")) { return OREOLED_BL_TARGET_AVR; - } else if (!strcmp(target, "auto") == 0) { + } else if (!strcmp(target, "auto")) { return OREOLED_BL_TARGET_DEFAULT; } @@ -102,7 +109,7 @@ oreoled_bl_parse_target(char *target) } int -oreoled_bl_main(int argc, char *argv[]) +oreoledbl_main(int argc, char *argv[]) { int i2cdevice = -1; int i2c_addr = OREOLED_BASE_I2C_ADDR; /* 7bit */ @@ -129,13 +136,13 @@ oreoled_bl_main(int argc, char *argv[]) warnx("invalid target '%s' specified", optarg); default: - oreoled_bl_usage(); + oreoled_bl_init_usage(); exit(0); } } if (optind >= argc) { - oreoled_bl_usage(); + oreoled_bl_init_usage(); exit(1); } @@ -144,7 +151,7 @@ oreoled_bl_main(int argc, char *argv[]) int ret; /* start driver */ - if (!strcmp(verb, "update")) { + if (!strcmp(verb, "start") || !strcmp(verb, "update")) { if (g_oreoled_bl != nullptr) { errx(1, "already started"); } @@ -154,17 +161,10 @@ oreoled_bl_main(int argc, char *argv[]) i2cdevice = PX4_I2C_BUS_LED; } - /* handle update flag */ - bool force_update = false; - if (argc > 2 && !strcmp(argv[2], "force")) { - warnx("forcing update"); - force_update = true; - } - /* instantiate driver */ switch (target) { case OREOLED_BL_TARGET_AVR: - g_oreoled_bl = new OREOLED_BOOTLOADER_AVR(i2cdevice, i2c_addr, force_update); + g_oreoled_bl = new OREOLED_BOOTLOADER_AVR(i2cdevice, i2c_addr); break; default: @@ -176,11 +176,29 @@ oreoled_bl_main(int argc, char *argv[]) errx(1, "failed to allocated memory for driver"); } - /* check object was created successfully */ - if (g_oreoled_bl->update() != OK) { + if (!strcmp(verb, "start")) { + /* check object was created successfully */ + if (g_oreoled_bl->start() != OK) { + delete g_oreoled_bl; + g_oreoled_bl = nullptr; + errx(1, "failed to start driver"); + } + } else { + /* handle update flag */ + bool force_update = false; + if (argc > optind && !strcmp(argv[optind + 1], "force")) { + warnx("forcing update"); + force_update = true; + } + + /* check object was created successfully */ + if (g_oreoled_bl->update(force_update) != OK) { + errx(1, "failed to start driver"); + } + + /* clean up the driver */ delete g_oreoled_bl; g_oreoled_bl = nullptr; - errx(1, "failed to start driver"); } exit(0); @@ -189,13 +207,13 @@ oreoled_bl_main(int argc, char *argv[]) /* need the driver past this point */ if (g_oreoled_bl == nullptr) { warnx("not started"); - oreoled_bl_usage(); + oreoled_bl_init_usage(); exit(1); } - if (!strcmp(verb, "kill")) { + if (!strcmp(verb, "stop")) { /* delete the oreoled object if stop was requested, in addition to turning off the LED. */ - if (!strcmp(verb, "kill")) { + if (!strcmp(verb, "stop")) { OREOLED_BOOTLOADER *tmp_oreoled = g_oreoled_bl; g_oreoled_bl = nullptr; delete tmp_oreoled; @@ -286,7 +304,7 @@ oreoled_bl_main(int argc, char *argv[]) } /* ask all LEDs for their bootloader version */ - if (!strcmp(verb, "blver")) { + if (!strcmp(verb, "ver")) { if (argc < 2) { errx(1, "Usage: oreoledbl ver"); } @@ -306,7 +324,7 @@ oreoled_bl_main(int argc, char *argv[]) } /* ask all LEDs for their application version */ - if (!strcmp(verb, "ver")) { + if (!strcmp(verb, "appver")) { if (argc < 2) { errx(1, "Usage: oreoledbl appver"); } @@ -369,6 +387,6 @@ oreoled_bl_main(int argc, char *argv[]) exit(ret); } - oreoled_bl_usage(); + oreoled_bl_cmd_usage(); exit(0); } diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h index 39e773dabeed..05facd5547de 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.h @@ -55,7 +55,8 @@ class OREOLED_BOOTLOADER OREOLED_BOOTLOADER() {}; virtual ~OREOLED_BOOTLOADER() {}; - virtual int update(void) = 0; + virtual int start(void) = 0; + virtual int update(bool force) = 0; virtual int ioctl(const unsigned cmd, const unsigned long arg) = 0; }; diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp index e2b88a4c52c9..119165496608 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp @@ -53,7 +53,6 @@ #include #include -#include #include #include @@ -71,7 +70,8 @@ #define OREOLED_BOOT_FLASH_WAITMS 10 -#define OREOLED_BOOT_SUPPORTED_VER 0x01 +#define OREOLED_BOOT_SUPPORTED_VER_MIN 0x01 +#define OREOLED_BOOT_SUPPORTED_VER_MAX 0x02 #define OREOLED_BOOT_CMD_PING 0x40 #define OREOLED_BOOT_CMD_BL_VER 0x41 @@ -104,13 +104,10 @@ #define OREOLED_UPDATE_INTERVAL_US (1000000U / 50U) ///< time in microseconds, run at 50hz /* constructor */ -OREOLED_BOOTLOADER_AVR::OREOLED_BOOTLOADER_AVR(int bus, int i2c_addr, bool force_update) : +OREOLED_BOOTLOADER_AVR::OREOLED_BOOTLOADER_AVR(int bus, int i2c_addr) : I2C("oreoledbl_avr", OREOLEDBL0_DEVICE_PATH, bus, i2c_addr, 100000), - _work{}, _num_healthy(0), _num_inboot(0), - _force_update(force_update), - _is_bootloading(false), _fw_checksum(0x0000), _probe_perf(perf_alloc(PC_ELAPSED, "oreoled_bl_probe")), _comms_errors(perf_alloc(PC_COUNT, "oreoled_bl_comms_errors")), @@ -136,7 +133,7 @@ OREOLED_BOOTLOADER_AVR::~OREOLED_BOOTLOADER_AVR() } int -OREOLED_BOOTLOADER_AVR::update(void) +OREOLED_BOOTLOADER_AVR::start(void) { int ret; @@ -148,7 +145,21 @@ OREOLED_BOOTLOADER_AVR::update(void) } startup_discovery(); - run_updates(); + + return OK; +} + +int +OREOLED_BOOTLOADER_AVR::update(bool force) +{ + int ret; + + ret = start(); + if (ret != OK) { + return ret; + } + + run_updates(force); print_info(); @@ -323,10 +334,9 @@ OREOLED_BOOTLOADER_AVR::discover(void) } void -OREOLED_BOOTLOADER_AVR::run_updates(void) +OREOLED_BOOTLOADER_AVR::run_updates(bool force) { - update_application(_force_update); - _force_update = false; + update_application(force); if (_num_inboot > 0) { boot_all(); @@ -371,7 +381,6 @@ OREOLED_BOOTLOADER_AVR::update_application(const bool force_update) int OREOLED_BOOTLOADER_AVR::app_reset(const int led_num) { - _is_bootloading = true; oreoled_cmd_t boot_cmd; boot_cmd.led_num = led_num; @@ -411,7 +420,6 @@ OREOLED_BOOTLOADER_AVR::app_reset(const int led_num) usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - _is_bootloading = false; return ret; } @@ -472,7 +480,6 @@ OREOLED_BOOTLOADER_AVR::app_ping(const int led_num) uint16_t OREOLED_BOOTLOADER_AVR::inapp_checksum(const int led_num) { - _is_bootloading = true; oreoled_cmd_t boot_cmd; boot_cmd.led_num = led_num; @@ -487,17 +494,16 @@ OREOLED_BOOTLOADER_AVR::inapp_checksum(const int led_num) boot_cmd.num_bytes = 3; cmd_add_checksum(&boot_cmd); + const uint8_t APP_CHECKSUM_REPLY_LENGTH = 6; uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, APP_CHECKSUM_REPLY_LENGTH); /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_PARAM_APP_CHECKSUM && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + if (response_is_valid(&boot_cmd, reply, APP_CHECKSUM_REPLY_LENGTH) == OK) { warnx("bl app checksum OK from LED %i", boot_cmd.led_num); warnx("bl app checksum msb: 0x%x", reply[3]); warnx("bl app checksum lsb: 0x%x", reply[4]); @@ -506,11 +512,7 @@ OREOLED_BOOTLOADER_AVR::inapp_checksum(const int led_num) } else { warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); - warnx("bl app checksum response ADDR: 0x%x", reply[1]); - warnx("bl app checksum response CMD: 0x%x", reply[2]); - warnx("bl app checksum response VER H: 0x%x", reply[3]); - warnx("bl app checksum response VER L: 0x%x", reply[4]); - warnx("bl app checksum response XOR: 0x%x", reply[5]); + print_response(reply, APP_CHECKSUM_REPLY_LENGTH); if (retry > 1) { warnx("bl app checksum retrying LED %i", boot_cmd.led_num); @@ -522,14 +524,12 @@ OREOLED_BOOTLOADER_AVR::inapp_checksum(const int led_num) } } - _is_bootloading = false; return ret; } int OREOLED_BOOTLOADER_AVR::ping(const int led_num) { - _is_bootloading = true; oreoled_cmd_t boot_cmd; boot_cmd.led_num = led_num; @@ -543,28 +543,24 @@ OREOLED_BOOTLOADER_AVR::ping(const int led_num) boot_cmd.num_bytes = 2; cmd_add_checksum(&boot_cmd); + const uint8_t BL_PING_REPLY_LENGTH = 5; uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, BL_PING_REPLY_LENGTH); /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_PING && - reply[3] == OREOLED_BOOT_CMD_PING_NONCE && - reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + if (response_is_valid(&boot_cmd, reply, BL_PING_REPLY_LENGTH) == OK && + reply[3] == OREOLED_BOOT_CMD_PING_NONCE) { warnx("bl ping OK from LED %i", boot_cmd.led_num); ret = OK; break; } else { warnx("bl ping FAIL from LED %i", boot_cmd.led_num); - warnx("bl ping response ADDR: 0x%x", reply[1]); - warnx("bl ping response CMD: 0x%x", reply[2]); - warnx("bl ping response NONCE: 0x%x", reply[3]); - warnx("bl ping response XOR: 0x%x", reply[4]); + print_response(reply, BL_PING_REPLY_LENGTH); if (retry > 1) { warnx("bl ping retrying LED %i", boot_cmd.led_num); @@ -576,14 +572,12 @@ OREOLED_BOOTLOADER_AVR::ping(const int led_num) } } - _is_bootloading = false; return ret; } uint8_t OREOLED_BOOTLOADER_AVR::version(const int led_num) { - _is_bootloading = true; oreoled_cmd_t boot_cmd; boot_cmd.led_num = led_num; @@ -597,27 +591,22 @@ OREOLED_BOOTLOADER_AVR::version(const int led_num) boot_cmd.num_bytes = 2; cmd_add_checksum(&boot_cmd); + const uint8_t BL_VER_REPLY_LENGTH = 5; uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, BL_VER_REPLY_LENGTH); /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_BL_VER && - reply[3] == OREOLED_BOOT_SUPPORTED_VER && - reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + if (response_is_valid(&boot_cmd, reply, BL_VER_REPLY_LENGTH) == OK && version_is_supported(reply[3]) == OK) { warnx("bl ver from LED %i = %i", boot_cmd.led_num, reply[3]); ret = reply[3]; break; - } else { - warnx("bl ver response ADDR: 0x%x", reply[1]); - warnx("bl ver response CMD: 0x%x", reply[2]); - warnx("bl ver response VER: 0x%x", reply[3]); - warnx("bl ver response XOR: 0x%x", reply[4]); + warnx("bl ver FAIL from LED %i", boot_cmd.led_num); + print_response(reply, BL_VER_REPLY_LENGTH); if (retry > 1) { warnx("bl ver retrying LED %i", boot_cmd.led_num); @@ -629,14 +618,22 @@ OREOLED_BOOTLOADER_AVR::version(const int led_num) } } - _is_bootloading = false; return ret; } +int +OREOLED_BOOTLOADER_AVR::version_is_supported(const uint8_t ver) +{ + if (ver >= OREOLED_BOOT_SUPPORTED_VER_MIN && ver <= OREOLED_BOOT_SUPPORTED_VER_MAX) { + return OK; + } + + return EINVAL; +} + uint16_t OREOLED_BOOTLOADER_AVR::app_version(const int led_num) { - _is_bootloading = true; oreoled_cmd_t boot_cmd; boot_cmd.led_num = led_num; @@ -650,17 +647,16 @@ OREOLED_BOOTLOADER_AVR::app_version(const int led_num) boot_cmd.num_bytes = 2; cmd_add_checksum(&boot_cmd); + const uint8_t BL_APP_VER_REPLY_LENGTH = 6; uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, BL_APP_VER_REPLY_LENGTH); /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_APP_VER && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + if (response_is_valid(&boot_cmd, reply, BL_APP_VER_REPLY_LENGTH) == OK) { warnx("bl app version OK from LED %i", boot_cmd.led_num); warnx("bl app version msb: 0x%x", reply[3]); warnx("bl app version lsb: 0x%x", reply[4]); @@ -669,11 +665,7 @@ OREOLED_BOOTLOADER_AVR::app_version(const int led_num) } else { warnx("bl app version FAIL from LED %i", boot_cmd.led_num); - warnx("bl app version response ADDR: 0x%x", reply[1]); - warnx("bl app version response CMD: 0x%x", reply[2]); - warnx("bl app version response VER H: 0x%x", reply[3]); - warnx("bl app version response VER L: 0x%x", reply[4]); - warnx("bl app version response XOR: 0x%x", reply[5]); + print_response(reply, BL_APP_VER_REPLY_LENGTH); if (retry > 1) { warnx("bl app version retrying LED %i", boot_cmd.led_num); @@ -685,14 +677,12 @@ OREOLED_BOOTLOADER_AVR::app_version(const int led_num) } } - _is_bootloading = false; return ret; } uint16_t OREOLED_BOOTLOADER_AVR::app_checksum(const int led_num) { - _is_bootloading = true; oreoled_cmd_t boot_cmd; boot_cmd.led_num = led_num; @@ -706,17 +696,16 @@ OREOLED_BOOTLOADER_AVR::app_checksum(const int led_num) boot_cmd.num_bytes = 2; cmd_add_checksum(&boot_cmd); + const uint8_t BL_APP_CHECKSUM_REPLY_LENGTH = 6; uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, BL_APP_CHECKSUM_REPLY_LENGTH); /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_APP_CHECKSUM && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + if (response_is_valid(&boot_cmd, reply, BL_APP_CHECKSUM_REPLY_LENGTH) == OK) { warnx("bl app checksum OK from LED %i", boot_cmd.led_num); warnx("bl app checksum msb: 0x%x", reply[3]); warnx("bl app checksum lsb: 0x%x", reply[4]); @@ -725,11 +714,7 @@ OREOLED_BOOTLOADER_AVR::app_checksum(const int led_num) } else { warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); - warnx("bl app checksum response ADDR: 0x%x", reply[1]); - warnx("bl app checksum response CMD: 0x%x", reply[2]); - warnx("bl app checksum response VER H: 0x%x", reply[3]); - warnx("bl app checksum response VER L: 0x%x", reply[4]); - warnx("bl app checksum response XOR: 0x%x", reply[5]); + print_response(reply, BL_APP_CHECKSUM_REPLY_LENGTH); if (retry > 1) { warnx("bl app checksum retrying LED %i", boot_cmd.led_num); @@ -741,14 +726,12 @@ OREOLED_BOOTLOADER_AVR::app_checksum(const int led_num) } } - _is_bootloading = false; return ret; } int OREOLED_BOOTLOADER_AVR::set_colour(const int led_num, const uint8_t red, const uint8_t green) { - _is_bootloading = true; oreoled_cmd_t boot_cmd; boot_cmd.led_num = led_num; @@ -764,26 +747,23 @@ OREOLED_BOOTLOADER_AVR::set_colour(const int led_num, const uint8_t red, const u boot_cmd.num_bytes = 4; cmd_add_checksum(&boot_cmd); + const uint8_t BL_COLOUR_REPLY_LENGTH = 4; uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, BL_COLOUR_REPLY_LENGTH); /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_SET_COLOUR && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + if (response_is_valid(&boot_cmd, reply, BL_COLOUR_REPLY_LENGTH) == OK) { warnx("bl set colour OK from LED %i", boot_cmd.led_num); ret = OK; break; } else { warnx("bl set colour FAIL from LED %i", boot_cmd.led_num); - warnx("bl set colour response ADDR: 0x%x", reply[1]); - warnx("bl set colour response CMD: 0x%x", reply[2]); - warnx("bl set colour response XOR: 0x%x", reply[3]); + print_response(reply, BL_COLOUR_REPLY_LENGTH); if (retry > 1) { warnx("bl app colour retrying LED %i", boot_cmd.led_num); @@ -795,14 +775,12 @@ OREOLED_BOOTLOADER_AVR::set_colour(const int led_num, const uint8_t red, const u } } - _is_bootloading = false; return ret; } int OREOLED_BOOTLOADER_AVR::flash(const int led_num) { - _is_bootloading = true; oreoled_cmd_t boot_cmd; boot_cmd.led_num = led_num; @@ -857,6 +835,7 @@ OREOLED_BOOTLOADER_AVR::flash(const int led_num) /* Set the current address */ set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + const uint8_t BL_FLASH_REPLY_LENGTH = 4; uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; /* Loop through flash pages */ @@ -874,20 +853,16 @@ OREOLED_BOOTLOADER_AVR::flash(const int led_num) for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, BL_FLASH_REPLY_LENGTH); /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_A && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + if (response_is_valid(&boot_cmd, reply, BL_FLASH_REPLY_LENGTH) == OK) { warnx("bl flash %ia OK for LED %i", page_idx, boot_cmd.led_num); break; } else { warnx("bl flash %ia FAIL for LED %i", page_idx, boot_cmd.led_num); - warnx("bl flash %ia response ADDR: 0x%x", page_idx, reply[1]); - warnx("bl flash %ia response CMD: 0x%x", page_idx, reply[2]); - warnx("bl flash %ia response XOR: 0x%x", page_idx, reply[3]); + print_response(reply, BL_FLASH_REPLY_LENGTH); if (retry > 1) { warnx("bl flash %ia retrying LED %i", page_idx, boot_cmd.led_num); @@ -911,20 +886,16 @@ OREOLED_BOOTLOADER_AVR::flash(const int led_num) for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write+Read */ memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, BL_FLASH_REPLY_LENGTH); /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_B && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + if (response_is_valid(&boot_cmd, reply, BL_FLASH_REPLY_LENGTH) == OK) { warnx("bl flash %ib OK for LED %i", page_idx, boot_cmd.led_num); break; } else { warnx("bl flash %ib FAIL for LED %i", page_idx, boot_cmd.led_num); - warnx("bl flash %ib response ADDR: 0x%x", page_idx, reply[1]); - warnx("bl flash %ib response CMD: 0x%x", page_idx, reply[2]); - warnx("bl flash %ib response XOR: 0x%x", page_idx, reply[3]); + print_response(reply, BL_FLASH_REPLY_LENGTH); if (retry > 1) { warnx("bl flash %ib retrying LED %i", page_idx, boot_cmd.led_num); @@ -965,19 +936,16 @@ OREOLED_BOOTLOADER_AVR::flash(const int led_num) for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES * 2; retry > 0; retry--) { /* Send the I2C Write */ memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); + transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, BL_FLASH_REPLY_LENGTH); /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_FINALISE_FLASH && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + if (response_is_valid(&boot_cmd, reply, BL_FLASH_REPLY_LENGTH) == OK) { warnx("bl finalise OK from LED %i", boot_cmd.led_num); break; } else { - warnx("bl finalise response ADDR: 0x%x", reply[1]); - warnx("bl finalise response CMD: 0x%x", reply[2]); - warnx("bl finalise response XOR: 0x%x", reply[3]); + warnx("bl flash finalise FAIL for LED %i", boot_cmd.led_num); + print_response(reply, BL_FLASH_REPLY_LENGTH); if (retry > 1) { warnx("bl finalise retrying LED %i", boot_cmd.led_num); @@ -997,14 +965,12 @@ OREOLED_BOOTLOADER_AVR::flash(const int led_num) /* clean up file buffer */ delete[] buf; - _is_bootloading = false; - return 1; + return OK; } int OREOLED_BOOTLOADER_AVR::boot(const int led_num) { - _is_bootloading = true; oreoled_cmd_t boot_cmd; boot_cmd.led_num = led_num; @@ -1019,22 +985,25 @@ OREOLED_BOOTLOADER_AVR::boot(const int led_num) boot_cmd.num_bytes = 3; cmd_add_checksum(&boot_cmd); + const uint8_t BL_BOOT_REPLY_LENGTH = 4; + uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; + for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { /* Send the I2C Write */ - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_BOOT_APP && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { + if (response_is_valid(&boot_cmd, reply, BL_BOOT_REPLY_LENGTH) == OK) { warnx("bl boot OK from LED %i", boot_cmd.led_num); - /* decrement the inboot counter so we don't get confused */ _in_boot[led_num] = false; _num_inboot--; ret = OK; break; + /* + * The bootloader will respond with OREOLED_BOOT_CMD_BOOT_NONCE in + * reply[2] if the boot operation failed + */ } else if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && reply[2] == OREOLED_BOOT_CMD_BOOT_NONCE && reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { @@ -1042,9 +1011,8 @@ OREOLED_BOOTLOADER_AVR::boot(const int led_num) break; } else { - warnx("bl boot response ADDR: 0x%x", reply[1]); - warnx("bl boot response CMD: 0x%x", reply[2]); - warnx("bl boot response XOR: 0x%x", reply[3]); + warnx("bl boot FAIL for LED %i", boot_cmd.led_num); + print_response(reply, BL_BOOT_REPLY_LENGTH); if (retry > 1) { warnx("bl boot retrying LED %i", boot_cmd.led_num); @@ -1062,7 +1030,6 @@ OREOLED_BOOTLOADER_AVR::boot(const int led_num) usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - _is_bootloading = false; return ret; } @@ -1152,11 +1119,11 @@ OREOLED_BOOTLOADER_AVR::firmware_checksum(void) int OREOLED_BOOTLOADER_AVR::flash_all(const bool force_update) { - int ret = -1; + int ret = OK; for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { if (_healthy[i] && _in_boot[i]) { - int result; + int result = EINVAL; if (force_update) { result = flash(i); @@ -1207,3 +1174,29 @@ OREOLED_BOOTLOADER_AVR::cmd_add_checksum(oreoled_cmd_t* cmd) cmd->buff[checksum_idx] ^= cmd->buff[i]; } } + +int +OREOLED_BOOTLOADER_AVR::response_is_valid(const oreoled_cmd_t* cmd, const uint8_t* response, const uint8_t response_len) +{ + if (response[1] == OREOLED_BASE_I2C_ADDR + cmd->led_num && + response[2] == cmd->buff[0] && + response[response_len - 1] == cmd->buff[cmd->num_bytes - 1]) { + return OK; + } + + return EINVAL; +} + +void +OREOLED_BOOTLOADER_AVR::print_response(const uint8_t* response, const uint8_t response_length) +{ + const uint8_t checksum_idx = response_length - 1; + warnx("bl ver response ADDR: 0x%x", response[1]); + warnx("bl ver response CMD: 0x%x", response[2]); + + for (uint8_t i = 3; i < checksum_idx; i++) { + warnx("bl ver response [%i]: 0x%x", i, response[3]); + } + + warnx("bl ver response XOR: 0x%x", response[checksum_idx]); +} \ No newline at end of file diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h index 9d44b3c4bb96..044dc6389f50 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.h @@ -44,17 +44,18 @@ class OREOLED_BOOTLOADER_AVR : public OREOLED_BOOTLOADER, public device::I2C { public: - OREOLED_BOOTLOADER_AVR(int bus, int i2c_addr, bool force_update); + OREOLED_BOOTLOADER_AVR(int bus, int i2c_addr); ~OREOLED_BOOTLOADER_AVR(); - int update(void); + int start(void); + int update(bool force); int ioctl(const unsigned cmd, const unsigned long arg); private: void print_info(void); void startup_discovery(void); void discover(void); - void run_updates(void); + void run_updates(bool force); void update_application(const bool force_update); int app_reset(const int led_num); @@ -63,6 +64,7 @@ class OREOLED_BOOTLOADER_AVR : public OREOLED_BOOTLOADER, public device::I2C uint16_t inapp_checksum(const int led_num); int ping(const int led_num); uint8_t version(const int led_num); + int version_is_supported(const uint8_t ver); uint16_t app_version(const int led_num); uint16_t app_checksum(const int led_num); int set_colour(const int led_num, const uint8_t red, const uint8_t green); @@ -73,16 +75,15 @@ class OREOLED_BOOTLOADER_AVR : public OREOLED_BOOTLOADER, public device::I2C uint16_t firmware_checksum(void); int coerce_healthy(void); void cmd_add_checksum(oreoled_cmd_t* cmd); + int response_is_valid(const oreoled_cmd_t* cmd, const uint8_t* response, const uint8_t response_len); + void print_response(const uint8_t* response, const uint8_t response_length); /* internal variables */ - work_s _work; ///< work queue for scheduling reads bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED bool _in_boot[OREOLED_NUM_LEDS]; ///< true for each LED that is in bootloader mode uint8_t _num_healthy; ///< number of healthy LEDs uint8_t _num_inboot; ///< number of LEDs in bootloader uint64_t _start_time; ///< system time we first attempt to communicate with battery - bool _force_update; ///< true if the driver should update all LEDs - bool _is_bootloading; ///< true if a bootloading operation is in progress uint16_t _fw_checksum; ///< the current 16bit XOR checksum of the built in oreoled firmware binary /* performance checking */ From 2c9284fbdc8f16362ee27975a7ab3b9fc54536e5 Mon Sep 17 00:00:00 2001 From: Angus Peart Date: Tue, 24 Nov 2015 10:01:11 +1100 Subject: [PATCH 287/293] oreoledbl: small fixes --- .../oreoled_bootloader/oreoled_bootloader.cpp | 16 ++++++---------- .../oreoled_bootloader_avr.cpp | 2 +- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp index 4f0cf0438e8f..efe5778ca872 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file oreoled.cpp + * @file oreoled_bootloader.cpp * @author Angus Peart */ @@ -129,7 +129,7 @@ oreoledbl_main(int argc, char *argv[]) break; case 't': - target = oreoled_bl_parse_target(strdup(optarg)); + target = oreoled_bl_parse_target(optarg); if (target != OREOLED_BL_TARGET_INVALID) { break; } @@ -213,14 +213,10 @@ oreoledbl_main(int argc, char *argv[]) if (!strcmp(verb, "stop")) { /* delete the oreoled object if stop was requested, in addition to turning off the LED. */ - if (!strcmp(verb, "stop")) { - OREOLED_BOOTLOADER *tmp_oreoled = g_oreoled_bl; - g_oreoled_bl = nullptr; - delete tmp_oreoled; - exit(0); - } - - exit(ret); + OREOLED_BOOTLOADER *tmp_oreoled = g_oreoled_bl; + g_oreoled_bl = nullptr; + delete tmp_oreoled; + exit(0); } /* send reset request to all LEDS */ diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp index 119165496608..94737a7f6015 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file oreoled.cpp + * @file oreoled_bootloader_avr.cpp * @author Angus Peart */ From ebfb5dcc9cf6c1dc80b80eac98b3de23640eee04 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 6 Jan 2016 17:36:52 -0800 Subject: [PATCH 288/293] oreoled: fix log statement --- src/drivers/oreoled/oreoled.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 6b2e590959c6..136b5fcbb2b5 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -363,7 +363,7 @@ OREOLED::startup_discovery(void) which indicates a response from firmwares >= 1.3 */ if (reply[1] == OREOLED_BASE_I2C_ADDR + cmd.led_num && reply[2] == (cmd_checksum + 1)) { - log("oreoled %u ok - in application", (unsigned)i); + DEVICE_LOG("oreoled %u ok - in application", (unsigned)i); _healthy[i] = true; _num_healthy++; } else { From 7afb54264c650a67afec4e826bf7d50fff0d3588 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 6 Jan 2016 17:45:25 -0800 Subject: [PATCH 289/293] oreoled: typo and comment fixes --- src/drivers/drv_oreoled_bootloader.h | 2 +- src/drivers/oreoled/oreoled.cpp | 4 ++-- .../oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/drv_oreoled_bootloader.h b/src/drivers/drv_oreoled_bootloader.h index 1b06c4cc56e0..120e95fff815 100644 --- a/src/drivers/drv_oreoled_bootloader.h +++ b/src/drivers/drv_oreoled_bootloader.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file drv_oreoled.h + * @file drv_oreoled_bootloader.h * @author Angus Peart * OreoLED bootloader device API */ diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index 136b5fcbb2b5..fdec574a45cf 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -77,7 +77,7 @@ #define OREOLED_CMD_QUEUE_SIZE 10 ///< up to 10 messages can be queued up to send to the LEDs /* magic number used to verify the software reset is valid */ -#define OEROLED_RESET_NONCE 0x2A +#define OREOLED_RESET_NONCE 0x2A class OREOLED : public device::I2C { @@ -472,7 +472,7 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) new_cmd.led_num = OREOLED_ALL_INSTANCES; new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; new_cmd.buff[1] = OREOLED_PARAM_RESET; - new_cmd.buff[2] = OEROLED_RESET_NONCE; + new_cmd.buff[2] = OREOLED_RESET_NONCE; new_cmd.num_bytes = 3; for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp index 94737a7f6015..868ff034a5f5 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader_avr.cpp @@ -89,7 +89,7 @@ #define OEROLED_BOOT_COMMAND_RETRIES 10 /* magic number used to verify the software reset is valid */ -#define OEROLED_RESET_NONCE 0x2A +#define OREOLED_RESET_NONCE 0x2A #define OREOLED_BOOT_CMD_PING_NONCE 0x2A #define OREOLED_BOOT_CMD_BOOT_NONCE 0xA2 @@ -392,7 +392,7 @@ OREOLED_BOOTLOADER_AVR::app_reset(const int led_num) /* send a reset */ boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; boot_cmd.buff[1] = OREOLED_PARAM_RESET; - boot_cmd.buff[2] = OEROLED_RESET_NONCE; + boot_cmd.buff[2] = OREOLED_RESET_NONCE; boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; boot_cmd.num_bytes = 4; cmd_add_checksum(&boot_cmd); From 7e3a8836e5573245cc1c16588cf409cc8a70e230 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 6 Jan 2016 19:27:55 -0800 Subject: [PATCH 290/293] cmake: add oreoled_bootloader --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v2_simple.cmake | 1 + cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + 3 files changed, 3 insertions(+) diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index c3a3586f66ee..92a918e9fec3 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -40,6 +40,7 @@ set(config_module_list #drivers/mkblctrl drivers/px4flow drivers/oreoled + drivers/oreoled/oreoled_bootloader drivers/gimbal drivers/pwm_input drivers/camera_trigger diff --git a/cmake/configs/nuttx_px4fmu-v2_simple.cmake b/cmake/configs/nuttx_px4fmu-v2_simple.cmake index 070fbd703fbb..825e756769e7 100644 --- a/cmake/configs/nuttx_px4fmu-v2_simple.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_simple.cmake @@ -39,6 +39,7 @@ set(config_module_list drivers/mkblctrl drivers/px4flow drivers/oreoled + drivers/oreoled/oreoled_bootloader drivers/gimbal drivers/pwm_input drivers/camera_trigger diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 850b1a55ea0a..6ca8bf0d7daf 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -37,6 +37,7 @@ set(config_module_list #drivers/mkblctrl drivers/px4flow drivers/oreoled + drivers/oreoled/oreoled_bootloader drivers/gimbal drivers/pwm_input drivers/camera_trigger From 4e861f966e43634bb16cf1f4bff52a553a445e1c Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 6 Jan 2016 19:28:25 -0800 Subject: [PATCH 291/293] oreoled: add CMakeLists.txt for oreoled_bootloader --- .../oreoled/oreoled_bootloader/CMakeLists.txt | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 src/drivers/oreoled/oreoled_bootloader/CMakeLists.txt diff --git a/src/drivers/oreoled/oreoled_bootloader/CMakeLists.txt b/src/drivers/oreoled/oreoled_bootloader/CMakeLists.txt new file mode 100644 index 000000000000..58b78674dee4 --- /dev/null +++ b/src/drivers/oreoled/oreoled_bootloader/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__oreoled__oreoled_bootloader + MAIN oreoledbl + COMPILE_FLAGS + -Os + SRCS + oreoled_bootloader.cpp + oreoled_bootloader_avr.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : From d181d324435d8a74c3bc1a4962f7906f6df3d242 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 11 Jan 2016 17:50:15 -0800 Subject: [PATCH 292/293] oreoled: use return instead of exit --- src/drivers/oreoled/oreoled.cpp | 30 +++++++++---------- .../oreoled_bootloader/oreoled_bootloader.cpp | 28 ++++++++--------- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index fdec574a45cf..dd8018a0a9c4 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -596,13 +596,13 @@ oreoled_main(int argc, char *argv[]) default: oreoled_usage(); - exit(0); + return 0; } } if (optind >= argc) { oreoled_usage(); - exit(1); + return 1; } const char *verb = argv[optind]; @@ -635,14 +635,14 @@ oreoled_main(int argc, char *argv[]) errx(1, "failed to start driver"); } - exit(0); + return 0; } /* need the driver past this point */ if (g_oreoled == nullptr) { warnx("not started"); oreoled_usage(); - exit(1); + return 1; } if (!strcmp(verb, "test")) { @@ -682,13 +682,13 @@ oreoled_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } /* display driver status */ if (!strcmp(verb, "info")) { g_oreoled->info(); - exit(0); + return 0; } if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { @@ -709,10 +709,10 @@ oreoled_main(int argc, char *argv[]) OREOLED *tmp_oreoled = g_oreoled; g_oreoled = nullptr; delete tmp_oreoled; - exit(0); + return 0; } - exit(ret); + return ret; } /* send rgb request to all LEDS */ @@ -737,7 +737,7 @@ oreoled_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } /* send macro request to all LEDS */ @@ -757,7 +757,7 @@ oreoled_main(int argc, char *argv[]) /* sanity check macro number */ if (macro > OREOLED_PARAM_MACRO_ENUM_COUNT) { errx(1, "invalid macro number %d", (int)macro); - exit(ret); + return ret; } oreoled_macrorun_t macro_run = {OREOLED_ALL_INSTANCES, (enum oreoled_macro)macro}; @@ -767,7 +767,7 @@ oreoled_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } /* send reset request to all LEDS */ @@ -787,14 +787,14 @@ oreoled_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } /* send general hardware call to all LEDS */ if (!strcmp(verb, "gencall")) { ret = g_oreoled->send_general_call(); warnx("sent general call"); - exit(ret); + return ret; } /* send a string of bytes to an LED using send_bytes function */ @@ -834,9 +834,9 @@ oreoled_main(int argc, char *argv[]) warnx("sent %d bytes", (int)sendb.num_bytes); } - exit(ret); + return ret; } oreoled_usage(); - exit(0); + return 0; } diff --git a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp index efe5778ca872..cdcbb1c1b9f2 100644 --- a/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp +++ b/src/drivers/oreoled/oreoled_bootloader/oreoled_bootloader.cpp @@ -137,13 +137,13 @@ oreoledbl_main(int argc, char *argv[]) default: oreoled_bl_init_usage(); - exit(0); + return 0; } } if (optind >= argc) { oreoled_bl_init_usage(); - exit(1); + return 1; } const char *verb = argv[optind]; @@ -201,14 +201,14 @@ oreoledbl_main(int argc, char *argv[]) g_oreoled_bl = nullptr; } - exit(0); + return 0; } /* need the driver past this point */ if (g_oreoled_bl == nullptr) { warnx("not started"); oreoled_bl_init_usage(); - exit(1); + return 1; } if (!strcmp(verb, "stop")) { @@ -216,7 +216,7 @@ oreoledbl_main(int argc, char *argv[]) OREOLED_BOOTLOADER *tmp_oreoled = g_oreoled_bl; g_oreoled_bl = nullptr; delete tmp_oreoled; - exit(0); + return 0; } /* send reset request to all LEDS */ @@ -236,7 +236,7 @@ oreoledbl_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } /* attempt to flash all LEDS in bootloader mode*/ @@ -256,7 +256,7 @@ oreoledbl_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } /* send bootloader boot request to all LEDS */ @@ -276,7 +276,7 @@ oreoledbl_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } /* send bootloader ping all LEDs */ @@ -296,7 +296,7 @@ oreoledbl_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } /* ask all LEDs for their bootloader version */ @@ -316,7 +316,7 @@ oreoledbl_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } /* ask all LEDs for their application version */ @@ -336,7 +336,7 @@ oreoledbl_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } /* ask all LEDs for their application checksum */ @@ -356,7 +356,7 @@ oreoledbl_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } /* set the default bootloader LED colour on all LEDs */ @@ -380,9 +380,9 @@ oreoledbl_main(int argc, char *argv[]) } close(fd); - exit(ret); + return ret; } oreoled_bl_cmd_usage(); - exit(0); + return 0; } From ac9fdeafbba65ae462e31e651c6f0884f45672cb Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 11 Jan 2016 17:56:53 -0800 Subject: [PATCH 293/293] batt_smbus: replace exit with return --- src/drivers/batt_smbus/batt_smbus.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 9321fad8b9f9..77ec418c2fd5 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -618,13 +618,13 @@ batt_smbus_main(int argc, char *argv[]) default: batt_smbus_usage(); - exit(0); + return 0; } } if (optind >= argc) { batt_smbus_usage(); - exit(1); + return 1; } const char *verb = argv[optind]; @@ -648,32 +648,32 @@ batt_smbus_main(int argc, char *argv[]) } } - exit(0); + return 0; } /* need the driver past this point */ if (g_batt_smbus == nullptr) { warnx("not started"); batt_smbus_usage(); - exit(1); + return 1; } if (!strcmp(verb, "test")) { g_batt_smbus->test(); - exit(0); + return 0; } if (!strcmp(verb, "stop")) { delete g_batt_smbus; g_batt_smbus = nullptr; - exit(0); + return 0; } if (!strcmp(verb, "search")) { g_batt_smbus->search(); - exit(0); + return 0; } batt_smbus_usage(); - exit(0); + return 0; }