From 71c87bb49e85b82a57c91e32229b5c6c5e170383 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 7 Nov 2014 10:37:40 +1100 Subject: [PATCH 001/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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/170] 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 586b69367c0a86ce100c8af00b56f949a8221080 Mon Sep 17 00:00:00 2001 From: Kevin Hester Date: Sun, 8 Mar 2015 12:58:15 -0700 Subject: [PATCH 170/170] otp: Add a system command for writing/viewing OTP, see docs below... 
nsh> otp
otp: usage:
otp show - show all block contents and lock bitsotp write  <32 hex bytes, LSB first, no spaces>  - will print values after readback
otp read  - will print 32 bytes readback from block (and the current CRC)
otp lock   - will permanently lock the specified block number

nsh> otp show
 0: L 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b
 1: L 3296c91156c6a2cd9d472bd768e74ad9c731d43b9180a994f9b927517e41425a e50fcf15
 2: L 5207fe5842dd9bcd27f2077759ca977292334e8c366969f3797efaaa6940ad00 26df18c8
 3: L c0190f185410e5fd13dce5b37b8f8030ab84e1fa1c026088eb1ee4b224fb662b ae616df5
 4: L a800a8110a956c55fe43698fa584e9a1d7f8b10495e3460c1de681c0c7b89875 3245288a
 5: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 6: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 7: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 8: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 9: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
10: U 5058340000002600001000000000000000000000000000000000000000000000 2fab057d
11: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
12: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
13: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
14: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
15: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6

nsh> otp write 10 ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
otp: Write not accepted

nsh> otp write 10 ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea67
otp: CRC does not match bytes

nsh> otp write 11 ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
WRITTEN

nsh> otp write 11 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b
WRITTEN

nsh> otp show
 0: L 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b
 1: L 3296c91156c6a2cd9d472bd768e74ad9c731d43b9180a994f9b927517e41425a e50fcf15
 2: L 5207fe5842dd9bcd27f2077759ca977292334e8c366969f3797efaaa6940ad00 26df18c8
 3: L c0190f185410e5fd13dce5b37b8f8030ab84e1fa1c026088eb1ee4b224fb662b ae616df5
 4: L a800a8110a956c55fe43698fa584e9a1d7f8b10495e3460c1de681c0c7b89875 3245288a
 5: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 6: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 7: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 8: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 9: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
10: U 5058340000002600001000000000000000000000000000000000000000000000 2fab057d
11: U 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b
12: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
13: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
14: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
15: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6

nsh> otp lock 10
otp: Invalid arguments
otp: usage:
otp show - show all block contents and lock bitsotp write  <32 hex bytes, LSB first, no spaces>  - will print values after readback
otp read  - will print 32 bytes readback from block (and the current CRC)
otp lock   - will permanently lock the specified block number

nsh> otp lock 10 10
LOCKED

nsh> otp show
 0: L 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b
 1: L 3296c91156c6a2cd9d472bd768e74ad9c731d43b9180a994f9b927517e41425a e50fcf15
 2: L 5207fe5842dd9bcd27f2077759ca977292334e8c366969f3797efaaa6940ad00 26df18c8
 3: L c0190f185410e5fd13dce5b37b8f8030ab84e1fa1c026088eb1ee4b224fb662b ae616df5
 4: L a800a8110a956c55fe43698fa584e9a1d7f8b10495e3460c1de681c0c7b89875 3245288a
 5: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 6: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 7: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 8: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
 9: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
10: L 5058340000002600001000000000000000000000000000000000000000000000 2fab057d
11: U 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b
12: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
13: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
14: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
15: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6
nsh>
--- src/modules/systemlib/otp.c | 25 +--- src/modules/systemlib/otp.h | 8 +- src/systemcmds/otp/README.txt | 96 +++++++++++++++ src/systemcmds/otp/module.mk | 43 +++++++ src/systemcmds/otp/otp.c | 217 ++++++++++++++++++++++++++++++++++ 5 files changed, 363 insertions(+), 26 deletions(-) create mode 100644 src/systemcmds/otp/README.txt create mode 100644 src/systemcmds/otp/module.mk create mode 100644 src/systemcmds/otp/otp.c diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index a866c843d7ca..fa9332d4e398 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -104,30 +104,9 @@ int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature) return errors; } -int lock_otp(void) +int lock_otp(int blocknum) { - //determine the required locking size - can only write full lock bytes */ -// int size = sizeof(struct otp) / 32; -// -// struct otp_lock otp_lock_mem; -// -// memset(&otp_lock_mem, OTP_LOCK_UNLOCKED, sizeof(otp_lock_mem)); -// for (int i = 0; i < sizeof(otp_lock_mem) / sizeof(otp_lock_mem.lock_bytes[0]); i++) -// otp_lock_mem.lock_bytes[i] = OTP_LOCK_LOCKED; - //XXX add the actual call here to write the OTP_LOCK bytes only at final stage - // val_copy(lock_ptr, &otp_lock_mem, sizeof(otp_lock_mem)); - - int locksize = 5; - - int errors = 0; - - // 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)) - errors++; - } - - return errors; + return F_write_byte(ADDR_OTP_LOCK_START + blocknum, OTP_LOCK_LOCKED); } diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h index 3ea0f80bd325..8b5fc4b609f5 100644 --- a/src/modules/systemlib/otp.h +++ b/src/modules/systemlib/otp.h @@ -51,7 +51,8 @@ __BEGIN_DECLS #define OTP_LOCK_LOCKED 0x00 #define OTP_LOCK_UNLOCKED 0xFF - +#define OTP_NUM_BLOCKS 16 +#define OTP_BLOCK_SIZE 32 #include #include @@ -91,7 +92,8 @@ typedef struct { #define F_KEY2 ((unsigned long)0xCDEF89AB) #define IS_F_ADDRESS(ADDRESS) ((((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x080FFFFF)) || (((ADDRESS) >= 0x1FFF7800) && ((ADDRESS) < 0x1FFF7A0F))) - +#define F_OTP_BLOCK_PTR(blocknum) ((volatile uint8_t *) (ADDR_OTP_START + OTP_BLOCK_SIZE * (blocknum))) +#define F_OTP_IS_LOCKED(blocknum) (((volatile uint8_t *) ADDR_OTP_LOCK_START)[blocknum] == 0) #pragma pack(push, 1) @@ -141,7 +143,7 @@ __EXPORT void F_lock(void); __EXPORT int val_read(void *dest, volatile const void *src, int bytes); __EXPORT int val_write(volatile void *dest, const void *src, int bytes); __EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature); -__EXPORT int lock_otp(void); +__EXPORT int lock_otp(int blocknum); __EXPORT int F_write_byte(unsigned long Address, uint8_t Data); diff --git a/src/systemcmds/otp/README.txt b/src/systemcmds/otp/README.txt new file mode 100644 index 000000000000..a7dc5e02f7f1 --- /dev/null +++ b/src/systemcmds/otp/README.txt @@ -0,0 +1,96 @@ + +nsh> otp +otp: usage: +otp show - show all block contents and lock bitsotp write <32 hex bytes, LSB first, no spaces> - will print values after readback +otp read - will print 32 bytes readback from block (and the current CRC) +otp lock - will permanently lock the specified block number + +# show is intended to be human readable +nsh> otp show + 0: L 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b + 1: L 3296c91156c6a2cd9d472bd768e74ad9c731d43b9180a994f9b927517e41425a e50fcf15 + 2: L 5207fe5842dd9bcd27f2077759ca977292334e8c366969f3797efaaa6940ad00 26df18c8 + 3: L c0190f185410e5fd13dce5b37b8f8030ab84e1fa1c026088eb1ee4b224fb662b ae616df5 + 4: L a800a8110a956c55fe43698fa584e9a1d7f8b10495e3460c1de681c0c7b89875 3245288a + 5: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 6: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 7: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 8: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 9: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +10: U 5058340000002600001000000000000000000000000000000000000000000000 2fab057d +11: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +12: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +13: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +14: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +15: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + +# the readback for writing to block 10 failed (OTP bits can _only_ be changed to zero) +# even if unlocked there is NO WAY to change the back to a one. So be careful even +# if testing +nsh> otp write 10 ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +otp: Write not accepted + +# Because of this, we check a crc32 that must be included with each write command +# if wrong we will not do the write +nsh> otp write 10 ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea67 +otp: CRC does not match bytes + +# The _only_ response that indicates success is WRITTEN +# This demonstrates writing ffs +nsh> otp write 11 ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +WRITTEN + +# This demonstrates writing some other string of bytes +nsh> otp write 11 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b +WRITTEN + +# See the happy bytes now on block 11 +nsh> otp show + 0: L 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b + 1: L 3296c91156c6a2cd9d472bd768e74ad9c731d43b9180a994f9b927517e41425a e50fcf15 + 2: L 5207fe5842dd9bcd27f2077759ca977292334e8c366969f3797efaaa6940ad00 26df18c8 + 3: L c0190f185410e5fd13dce5b37b8f8030ab84e1fa1c026088eb1ee4b224fb662b ae616df5 + 4: L a800a8110a956c55fe43698fa584e9a1d7f8b10495e3460c1de681c0c7b89875 3245288a + 5: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 6: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 7: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 8: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 9: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +10: U 5058340000002600001000000000000000000000000000000000000000000000 2fab057d +11: U 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b +12: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +13: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +14: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +15: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + +# To prevent locking incorrect blocks, the block number must be given twice +nsh> otp lock 10 +otp: Invalid arguments +otp: usage: +otp show - show all block contents and lock bitsotp write <32 hex bytes, LSB first, no spaces> - will print values after readback +otp read - will print 32 bytes readback from block (and the current CRC) +otp lock - will permanently lock the specified block number + +# The _only_ response that indicates success for the lock command is LOCKED +nsh> otp lock 10 10 +LOCKED + +# Notice that block 10 is now locked +nsh> otp show + 0: L 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b + 1: L 3296c91156c6a2cd9d472bd768e74ad9c731d43b9180a994f9b927517e41425a e50fcf15 + 2: L 5207fe5842dd9bcd27f2077759ca977292334e8c366969f3797efaaa6940ad00 26df18c8 + 3: L c0190f185410e5fd13dce5b37b8f8030ab84e1fa1c026088eb1ee4b224fb662b ae616df5 + 4: L a800a8110a956c55fe43698fa584e9a1d7f8b10495e3460c1de681c0c7b89875 3245288a + 5: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 6: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 7: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 8: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 + 9: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +10: L 5058340000002600001000000000000000000000000000000000000000000000 2fab057d +11: U 5058340000ac26000010000000ffffffffffffffffffffffffffffffffffffff 55393c7b +12: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +13: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +14: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +15: U ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff e666fea6 +nsh> diff --git a/src/systemcmds/otp/module.mk b/src/systemcmds/otp/module.mk new file mode 100644 index 000000000000..61adb8c0f287 --- /dev/null +++ b/src/systemcmds/otp/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# OTP program/read/lock utility. +# + +MODULE_COMMAND = otp +SRCS = otp.c + +MODULE_STACKSIZE = 1800 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/otp/otp.c b/src/systemcmds/otp/otp.c new file mode 100644 index 000000000000..aa929433a9fc --- /dev/null +++ b/src/systemcmds/otp/otp.c @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: @author Kevin Hester + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 otp.c + * Tool for reading/writing/locking OTP + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +__EXPORT int otp_main(int argc, char *argv[]); + + + +static void +usage(const char *reason) +{ + if (reason != NULL) { + warnx("%s", reason); + } + + errx(1, + "usage:\n" + "otp show - show all block contents and lock bits" + "otp write <32 hex bytes, LSB first, no spaces> - will print values after readback\n" + "otp read - will print 32 bytes readback from block (and the current CRC)\n" + "otp lock - will permanently lock the specified block number\n" + ); +} + +static void otp_lock(int blocknum) +{ + F_unlock(); + sched_lock(); + + lock_otp(blocknum); + + F_lock(); + sched_unlock(); +} + +static int otp_read(int i) +{ + printf("%2d: %c ", i, F_OTP_IS_LOCKED(i) ? 'L' : 'U'); + volatile uint8_t *base = F_OTP_BLOCK_PTR(i); + + for (int j = 0; j < OTP_BLOCK_SIZE; j++) { + printf("%02x", base[j]); + // if(j % 4 == 0) printf(" "); + } + + printf(" %08x\n", crc32((const uint8_t *) base, OTP_BLOCK_SIZE)); + return 0; +} + +static int otp_show(void) +{ + for (int i = 0; i < OTP_NUM_BLOCKS; i++) { + otp_read(i); + } + + return 0; +} + +static int otp_write(int blocknum, const char *hexstr, uint32_t expectedcrc) +{ + if (strlen(hexstr) != OTP_BLOCK_SIZE * 2) { + errx(1, "Bad hex string length"); + return 1; + } + + uint8_t bytes[OTP_BLOCK_SIZE]; + + for (int i = 0; i < OTP_BLOCK_SIZE; i++) { + char temp[3]; + temp[0] = hexstr[2 * i]; + temp[1] = hexstr[2 * i + 1]; + temp[2] = '\0'; + bytes[i] = (uint8_t) strtol(temp, NULL, 16); + } + + if (crc32(bytes, OTP_BLOCK_SIZE) != expectedcrc) { + errx(1, "CRC does not match bytes"); + return 1; + } + + F_unlock(); + sched_lock(); + + volatile uint8_t *base = F_OTP_BLOCK_PTR(blocknum); + + for (int i = 0; i < OTP_BLOCK_SIZE; i++) { + if (F_write_byte(ADDR_OTP_START + OTP_BLOCK_SIZE * blocknum + i, bytes[i])) { + errx(1, "Failed to write"); + return 1; + } + + if (base[i] != bytes[i]) { + errx(1, "Write not accepted"); + return 1; + } + } + + F_lock(); + sched_unlock(); + + printf("WRITTEN\n"); + return 0; +} + +int otp_main(int argc, char *argv[]) +{ + int getblocknum(int argnum) { + if (argnum >= argc) { + errx(1, "must specify a block number"); + return -1; + + } else { + int bnum = -1; + sscanf(argv[argnum], "%d", &bnum); + + if (bnum < 0 || bnum >= OTP_NUM_BLOCKS) { + errx(1, "invalid block number"); + return -1; + + } else { + return bnum; + } + } + } + + if (argc < 2) { + usage(NULL); + + } else { + const char *cmd = argv[1]; + + if (!strcmp(cmd, "show")) { + return otp_show(); + + } else if (!strcmp(cmd, "read") && argc == 3) { + int bnum = getblocknum(2); + + if (bnum >= 0) { + return otp_read(bnum); + } + + } else if (!strcmp(cmd, "write") && argc == 5) { + int bnum = getblocknum(2); + uint32_t expectedcrc = strtol(argv[4], NULL, 16); + + if (bnum >= 0) { + return otp_write(bnum, argv[3], expectedcrc); + } + + } else if (!strcmp(cmd, "lock") && argc == 4) { + int bnum = getblocknum(2); + int bnum2 = getblocknum(3); + + if (bnum >= 0 && bnum == bnum2) { + otp_lock(bnum); + printf("LOCKED\n"); + return 0; + + } else { + printf("FAILED\n"); + return 1; + } + + } else { + usage("Invalid arguments"); + } + } + + return 0; +}