diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-05 10:02:07 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-05 10:02:07 +0100 |
commit | 16b9f666e790a2b939f7890b39cc6e4cf2552165 (patch) | |
tree | ee936b06d15e75e12b175f6786772781aff32cd8 /src/drivers | |
parent | e16c4ff76e7eff2da88bcbef5d05dd4ba11e7203 (diff) | |
parent | c3ed35f5cc8e2617e61747e904dbaa652d1cc2c8 (diff) | |
download | px4-firmware-16b9f666e790a2b939f7890b39cc6e4cf2552165.tar.gz px4-firmware-16b9f666e790a2b939f7890b39cc6e4cf2552165.tar.bz2 px4-firmware-16b9f666e790a2b939f7890b39cc6e4cf2552165.zip |
Merge remote-tracking branch 'upstream/master' into dev_ros
Conflicts:
src/lib/mathlib/math/Matrix.hpp
src/modules/mc_att_control/mc_att_control_main.cpp
src/modules/uORB/topics/vehicle_status.h
src/platforms/px4_includes.h
Diffstat (limited to 'src/drivers')
27 files changed, 2264 insertions, 540 deletions
diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp new file mode 100644 index 000000000..2b5fef4d7 --- /dev/null +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -0,0 +1,585 @@ +/**************************************************************************** + * + * 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 batt_smbus.cpp + * + * Driver for a battery monitor connected via SMBus (I2C). + * + * @author Randy Mackay <rmackay9@yahoo.com> + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <sched.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> +#include <ctype.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <board_config.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include <uORB/uORB.h> +#include <uORB/topics/subsystem_info.h> +#include <uORB/topics/battery_status.h> + +#include <float.h> + +#include <drivers/device/i2c.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_batt_smbus.h> +#include <drivers/device/ringbuffer.h> + +#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 +#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_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 + +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(); + + /** + * Initialize device + * + * Calls probe() to check for device on bus. + * + * @return 0 on success, error code on failure + */ + virtual int init(); + + /** + * Test device + * + * @return 0 on success, error code on failure + */ + virtual int test(); + + /** + * Search all possible slave addresses for a smart battery + */ + int search(); + +protected: + /** + * Check if the device can be contacted + */ + virtual int probe(); + +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 a word from specified register + */ + int read_reg(uint8_t reg, uint16_t &val); + + /** + * Read block from bus + * @return 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); + + /** + * Calculate PEC for a read or write from the battery + * @param 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 + 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 +}; + +namespace +{ +BATT_SMBUS *g_batt_smbus; ///< device handle. For now, we only support one BATT_SMBUS device +} + +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), + _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)); +} + +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::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() +{ + // always return OK to ensure device starts + return OK; +} + +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); +} + +void +BATT_SMBUS::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +BATT_SMBUS::cycle_trampoline(void *arg) +{ + BATT_SMBUS *dev = (BATT_SMBUS *)arg; + + dev->cycle(); +} + +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; + + // 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) { + 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[3]; // 2 bytes of data + PEC + + // read from register + int ret = transfer(®, 1, buff, 3); + + if (ret == OK) { + // 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]; + + } else { + ret = ENOTTY; + } + } + + // return success or failure + return ret; +} + +uint8_t +BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero) +{ + uint8_t buff[max_len + 2]; // buffer to hold results + + usleep(1); + + // read bytes including PEC + int ret = transfer(®, 1, buff, max_len + 2); + + // 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; + } + + // 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; + + } + + // copy data + memcpy(data, &buff[1], bufflen); + + // optionally add zero to end + if (append_zero) { + data[bufflen] = '\0'; + } + + // return success + return bufflen; +} + +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 < sizeof(tmp_buff); i++) { + // load next data byte into the shift register + shift_reg = tmp_buff[i]; + + // for each bit in the current byte + for (uint8_t j = 0; j < 8; j++) { + do_invert = (crc ^ shift_reg) & 0x80; + crc <<= 1; + shift_reg <<= 1; + + if (do_invert) { + crc ^= BATT_SMBUS_PEC_POLYNOMIAL; + } + } + } + + // return result + return crc; +} + +///////////////////////// shell functions /////////////////////// + +void +batt_smbus_usage() +{ + 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); +} + +int +batt_smbus_main(int argc, char *argv[]) +{ + int i2cdevice = BATT_SMBUS_I2C_BUS; + int batt_smbusadr = BATT_SMBUS_ADDR; // 7bit address + + 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); + } + + if (!strcmp(verb, "search")) { + g_batt_smbus->search(); + 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 000000000..b32ea6e55 --- /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/device/device.h b/src/drivers/device/device.h index 67aaa0aff..4d4bed835 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -445,6 +445,13 @@ protected: */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char* get_devname() { return _devname; } + bool _pub_blocked; /**< true if publishing should be blocked */ private: diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 705b398b0..8518596ea 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -58,7 +58,7 @@ public: /** * Get the address */ - int16_t get_address() { return _address; } + int16_t get_address() const { return _address; } protected: /** @@ -132,6 +132,7 @@ protected: */ void set_address(uint16_t address) { _address = address; + _device_id.devid_s.address = _address; } private: diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 248b9a73d..3e28d3d3d 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -65,6 +65,7 @@ struct baro_report { */ ORB_DECLARE(sensor_baro0); ORB_DECLARE(sensor_baro1); +ORB_DECLARE(sensor_baro2); /* * ioctl() definitions diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h new file mode 100644 index 000000000..ca130c84e --- /dev/null +++ b/src/drivers/drv_batt_smbus.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 <stdint.h> +#include <sys/ioctl.h> +#include "drv_orb_dev.h" + +/* device path */ +#define BATT_SMBUS_DEVICE_PATH "/dev/batt_smbus" diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index ab640837b..5aed3f02b 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file Rangefinder driver interface. + * @file PX4Flow driver interface. */ #ifndef _DRV_PX4FLOW_H diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 8e8b2c1b9..5e4598de8 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -82,8 +82,19 @@ #define SENSORIOCGQUEUEDEPTH _SENSORIOC(3) /** - * Reset the sensor to its default configuration. + * Reset the sensor to its default configuration */ #define SENSORIOCRESET _SENSORIOC(4) -#endif /* _DRV_SENSOR_H */
\ No newline at end of file +/** + * Set the sensor orientation + */ +#define SENSORIOCSROTATION _SENSORIOC(5) + +/** + * Get the sensor orientation + */ +#define SENSORIOCGROTATION _SENSORIOC(6) + +#endif /* _DRV_SENSOR_H */ + diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index dd9b90fb3..890fada16 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -223,7 +223,7 @@ void frsky_send_frame2(int uart) char lat_ns = 0, lon_ew = 0; int sec = 0; if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { - time_t time_gps = global_pos.time_gps_usec / 1000000; + time_t time_gps = global_pos.time_utc_usec / 1000000ULL; struct tm *tm_gps = gmtime(&time_gps); course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; @@ -274,7 +274,7 @@ void frsky_send_frame3(int uart) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); /* send formatted frame */ - time_t time_gps = global_pos.time_gps_usec / 1000000; + time_t time_gps = global_pos.time_utc_usec / 1000000ULL; struct tm *tm_gps = gmtime(&time_gps); uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index a2e292de2..a50767c22 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -38,7 +38,7 @@ ASHTECH::~ASHTECH() int ASHTECH::handle_message(int len) { char * endp; - + if (len < 7) { return 0; } int uiCalcComma = 0; @@ -99,8 +99,26 @@ int ASHTECH::handle_message(int len) timeinfo.tm_sec = int(ashtech_sec); time_t epoch = mktime(&timeinfo); - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); + if (epoch > GPS_EPOCH_SECS) { + uint64_t usecs = static_cast<uint64_t>((ashtech_sec - static_cast<uint64_t>(ashtech_sec))) * 1e6; + + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = usecs * 1000; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } + + _gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL; + _gps_position->time_utc_usec += usecs; + } else { + _gps_position->time_utc_usec = 0; + } + _gps_position->timestamp_time = hrt_absolute_time(); } @@ -611,8 +629,8 @@ void ASHTECH::decode_init(void) } -/* - * ashtech board configuration script +/* + * ashtech board configuration script */ const char comm[] = "$PASHS,POP,20\r\n"\ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 47c907bd3..8d7176791 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -552,7 +552,7 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info) fd = open(GPS_DEVICE_PATH, O_RDONLY); if (fd < 0) { - errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); + errx(1, "open: %s\n", GPS_DEVICE_PATH); goto fail; } @@ -565,7 +565,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "start failed"); } /** @@ -604,7 +604,7 @@ reset() err(1, "failed "); if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); + err(1, "reset failed"); exit(0); } @@ -616,7 +616,7 @@ void info() { if (g_dev == nullptr) - errx(1, "driver not running"); + errx(1, "not running"); g_dev->print_info(); diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 3b92f1bf4..05dfc99b7 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -91,7 +91,7 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) warnx("try baudrate: %d\n", speed); default: - warnx("ERROR: Unsupported baudrate: %d\n", baud); + warnx("ERR: baudrate: %d\n", baud); return -EINVAL; } @@ -109,20 +109,19 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) /* set baud rate */ if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); + warnx("ERR: %d (cfsetispeed)\n", termios_state); return -1; } if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); + warnx("ERR: %d (cfsetospeed)\n", termios_state); return -1; } if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate (tcsetattr)\n"); + warnx("ERR: %d (tcsetattr)\n", termios_state); return -1; } - /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ return 0; } diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index 3623397b2..0ce05fcb5 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -43,6 +43,8 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_gps_position.h> +#define GPS_EPOCH_SECS 1234567890ULL + class GPS_Helper { public: diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index c4f4f7bec..c0c47073b 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -73,39 +73,38 @@ MTK::configure(unsigned &baudrate) /* Write config messages, don't wait for an answer */ if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } return 0; + +errout: + warnx("mtk: config write failed"); + return -1; } int @@ -222,7 +221,6 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) ret = 1; } else { - warnx("MTK Checksum invalid"); ret = -1; } @@ -282,8 +280,8 @@ MTK::handle_message(gps_mtk_packet_t &packet) timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; time_t epoch = mktime(&timeinfo); - _gps_position->time_gps_usec = epoch * 1e6; //TODO: test this - _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; + _gps_position->time_utc_usec = epoch * 1e6; //TODO: test this + _gps_position->time_utc_usec += timeinfo_conversion_temp * 1e3; _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); // Position and velocity update always at the same time diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b0eb4ab66..e197059db 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -748,17 +748,23 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; time_t epoch = mktime(&timeinfo); -#ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; - clock_settime(CLOCK_REALTIME, &ts); -#endif + if (epoch > GPS_EPOCH_SECS) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f); + _gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { + _gps_position->time_utc_usec = 0; + } } _gps_position->timestamp_time = hrt_absolute_time(); @@ -808,7 +814,7 @@ UBX::payload_rx_done(void) UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); { - /* convert to unix timestamp */ + // convert to unix timestamp struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900; timeinfo.tm_mon = _buf.payload_rx_nav_timeutc.month - 1; @@ -818,17 +824,25 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; time_t epoch = mktime(&timeinfo); -#ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; - clock_settime(CLOCK_REALTIME, &ts); -#endif + // only set the time if it makes sense + + if (epoch > GPS_EPOCH_SECS) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f); + _gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { + _gps_position->time_utc_usec = 0; + } } _gps_position->timestamp_time = hrt_absolute_time(); diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d4dbf3778..fe70bd37f 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,7 +34,7 @@ /** * @file hmc5883.cpp * - * Driver for the HMC5883 magnetometer connected via I2C. + * Driver for the HMC5883 / HMC5983 magnetometer connected via I2C or SPI. */ #include <nuttx/config.h> @@ -74,11 +74,12 @@ #include <getopt.h> #include <lib/conversion/rotation.h> +#include "hmc5883.h" + /* * HMC5883 internal constants and data structures. */ -#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 #define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int" #define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext" @@ -95,9 +96,6 @@ #define ADDR_DATA_OUT_Y_MSB 0x07 #define ADDR_DATA_OUT_Y_LSB 0x08 #define ADDR_STATUS 0x09 -#define ADDR_ID_A 0x0a -#define ADDR_ID_B 0x0b -#define ADDR_ID_C 0x0c /* modes not changeable outside of driver */ #define HMC5883L_MODE_NORMAL (0 << 0) /* default */ @@ -115,10 +113,11 @@ #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 ID_A_WHO_AM_I 'H' -#define ID_B_WHO_AM_I '4' -#define ID_C_WHO_AM_I '3' - +enum HMC5883_BUS { + HMC5883_BUS_ALL, + HMC5883_BUS_INTERNAL, + HMC5883_BUS_EXTERNAL +}; /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -130,10 +129,10 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class HMC5883 : public device::I2C +class HMC5883 : public device::CDev { public: - HMC5883(int bus, const char *path, enum Rotation rotation); + HMC5883(device::Device *interface, const char *path, enum Rotation rotation); virtual ~HMC5883(); virtual int init(); @@ -147,7 +146,7 @@ public: void print_info(); protected: - virtual int probe(); + Device *_interface; private: work_s _work; @@ -174,7 +173,6 @@ private: bool _sensor_ok; /**< sensor was found and reports ok */ bool _calibrated; /**< the calibration is valid */ - int _bus; /**< the bus the device is connected to */ enum Rotation _rotation; struct mag_report _last_report; /**< used for info() */ @@ -183,15 +181,6 @@ private: uint8_t _conf_reg; /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - - /** * Initialise the automatic measurement state machine and start it. * * @note This function is called at open and error time. It might make sense @@ -349,8 +338,9 @@ private: extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); -HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : - I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), +HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) : + CDev("HMC5883", path), + _interface(interface), _work{}, _measure_ticks(0), _reports(nullptr), @@ -369,7 +359,6 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")), _sensor_ok(false), _calibrated(false), - _bus(bus), _rotation(rotation), _last_report{0}, _range_bits(0), @@ -416,9 +405,11 @@ HMC5883::init() { int ret = ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) + ret = CDev::init(); + if (ret != OK) { + debug("CDev init failed"); goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(mag_report)); @@ -429,20 +420,7 @@ HMC5883::init() reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); - - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - _mag_orb_id = ORB_ID(sensor_mag0); - break; - - case CLASS_DEVICE_SECONDARY: - _mag_orb_id = ORB_ID(sensor_mag1); - break; - - case CLASS_DEVICE_TERTIARY: - _mag_orb_id = ORB_ID(sensor_mag2); - break; - } + _mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance); ret = OK; /* sensor is ok, but not calibrated */ @@ -559,30 +537,6 @@ void HMC5883::check_conf(void) } } -int -HMC5883::probe() -{ - uint8_t data[3] = {0, 0, 0}; - - _retries = 10; - - if (read_reg(ADDR_ID_A, data[0]) || - read_reg(ADDR_ID_B, data[1]) || - read_reg(ADDR_ID_C, data[2])) - debug("read_reg fail"); - - _retries = 2; - - if ((data[0] != ID_A_WHO_AM_I) || - (data[1] != ID_B_WHO_AM_I) || - (data[2] != ID_C_WHO_AM_I)) { - debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); - return -EIO; - } - - return OK; -} - ssize_t HMC5883::read(struct file *filp, char *buffer, size_t buflen) { @@ -643,6 +597,8 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) int HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) { + unsigned dummy = arg; + switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { @@ -768,14 +724,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return check_calibration(); case MAGIOCGEXTERNAL: - if (_bus == PX4_I2C_BUS_EXPANSION) - return 1; - else - return 0; + debug("MAGIOCGEXTERNAL in main driver"); + return _interface->ioctl(cmd, dummy); default: /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } } @@ -890,7 +844,6 @@ HMC5883::collect() } report; int ret; - uint8_t cmd; uint8_t check_counter; perf_begin(_sample_perf); @@ -908,8 +861,7 @@ HMC5883::collect() */ /* get measurements from the device */ - cmd = ADDR_DATA_OUT_X_MSB; - ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); + ret = _interface->read(ADDR_DATA_OUT_X_MSB, (uint8_t *)&hmc_report, sizeof(hmc_report)); if (ret != OK) { perf_count(_comms_errors); @@ -946,14 +898,14 @@ HMC5883::collect() /* scale values for output */ -#ifdef PX4_I2C_BUS_ONBOARD - if (_bus == PX4_I2C_BUS_ONBOARD) { + // XXX revisit for SPI part, might require a bus type IOCTL + unsigned dummy; + if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; report.x = -report.x; } -#endif /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x @@ -1291,15 +1243,17 @@ int HMC5883::set_excitement(unsigned enable) int HMC5883::write_reg(uint8_t reg, uint8_t val) { - uint8_t cmd[] = { reg, val }; - - return transfer(&cmd[0], 2, nullptr, 0); + uint8_t buf = val; + return _interface->write(reg, &buf, 1); } int HMC5883::read_reg(uint8_t reg, uint8_t &val) { - return transfer(®, 1, &val, 1); + uint8_t buf = val; + int ret = _interface->read(reg, &buf, 1); + val = buf; + return ret; } float @@ -1351,6 +1305,7 @@ void test(int bus); void reset(int bus); int info(int bus); int calibrate(int bus); +const char* get_path(int bus); void usage(); /** @@ -1360,43 +1315,99 @@ void usage(); * is either successfully up and running or failed to start. */ void -start(int bus, enum Rotation rotation) +start(int external_bus, enum Rotation rotation) { int fd; /* create the driver, attempt expansion bus first */ - if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { - if (g_dev_ext != nullptr) - errx(0, "already started external"); - g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation); - if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { - delete g_dev_ext; - g_dev_ext = nullptr; + 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; + } + } } -#ifdef PX4_I2C_BUS_ONBOARD /* if this failed, attempt onboard sensor */ - if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) - errx(0, "already started internal"); - g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); - if (g_dev_int != nullptr && OK != g_dev_int->init()) { + if (g_dev_int != nullptr) { + warnx("already started internal"); + } else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) { - /* tear down the failing onboard instance */ - delete g_dev_int; - g_dev_int = nullptr; + device::Device *interface = nullptr; - if (bus == PX4_I2C_BUS_ONBOARD) { - goto fail; - } + /* 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); } - if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { +#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 (interface->init() != OK) { + delete interface; + warnx("no device on internal bus"); + } else { + + 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; + + if (external_bus == HMC5883_BUS_INTERNAL) { + goto fail; + } + } + if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) { + goto fail; + } + } } -#endif if (g_dev_int == nullptr && g_dev_ext == nullptr) goto fail; @@ -1425,11 +1436,11 @@ start(int bus, enum Rotation rotation) exit(0); fail: - if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { + if (g_dev_int != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL)) { delete g_dev_int; g_dev_int = nullptr; } - if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { + if (g_dev_ext != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL)) { delete g_dev_ext; g_dev_ext = nullptr; } @@ -1448,7 +1459,7 @@ test(int bus) struct mag_report report; ssize_t sz; int ret; - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1549,7 +1560,7 @@ test(int bus) int calibrate(int bus) { int ret; - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1576,7 +1587,7 @@ int calibrate(int bus) void reset(int bus) { - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1600,12 +1611,12 @@ info(int bus) { int ret = 1; - HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + HMC5883 *g_dev = (bus == HMC5883_BUS_INTERNAL ? g_dev_int : g_dev_ext); if (g_dev == nullptr) { warnx("not running on bus %d", bus); } else { - warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard")); + warnx("running on bus: %d (%s)\n", bus, ((HMC5883_BUS_INTERNAL) ? "onboard" : "offboard")); g_dev->print_info(); ret = 0; @@ -1614,6 +1625,12 @@ info(int bus) return ret; } +const char* +get_path(int bus) +{ + return ((bus == HMC5883_BUS_INTERNAL) ? HMC5883L_DEVICE_PATH_INT : HMC5883L_DEVICE_PATH_EXT); +} + void usage() { @@ -1622,7 +1639,7 @@ usage() warnx(" -R rotation"); warnx(" -C calibrate on start"); warnx(" -X only external bus"); -#ifdef PX4_I2C_BUS_ONBOARD +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) warnx(" -I only internal bus"); #endif } @@ -1633,7 +1650,7 @@ int hmc5883_main(int argc, char *argv[]) { int ch; - int bus = -1; + int bus = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; @@ -1642,13 +1659,13 @@ hmc5883_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(optarg); break; -#ifdef PX4_I2C_BUS_ONBOARD +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) case 'I': - bus = PX4_I2C_BUS_ONBOARD; + bus = HMC5883_BUS_INTERNAL; break; #endif case 'X': - bus = PX4_I2C_BUS_EXPANSION; + bus = HMC5883_BUS_EXTERNAL; break; case 'C': calibrate = true; @@ -1692,13 +1709,13 @@ hmc5883_main(int argc, char *argv[]) * Print driver information. */ if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - if (bus == -1) { + if (bus == HMC5883_BUS_ALL) { int ret = 0; - if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) { + if (hmc5883::info(HMC5883_BUS_INTERNAL)) { ret = 1; } - if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) { + if (hmc5883::info(HMC5883_BUS_EXTERNAL)) { ret = 1; } exit(ret); diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h new file mode 100644 index 000000000..0eb773629 --- /dev/null +++ b/src/drivers/hmc5883/hmc5883.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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 hmc5883.h + * + * Shared defines for the hmc5883 driver. + */ + +#pragma once + +#define ADDR_ID_A 0x0a +#define ADDR_ID_B 0x0b +#define ADDR_ID_C 0x0c + +#define ID_A_WHO_AM_I 'H' +#define ID_B_WHO_AM_I '4' +#define ID_C_WHO_AM_I '3' + +/* interface factories */ +extern device::Device *HMC5883_SPI_interface(int bus) weak_function; +extern device::Device *HMC5883_I2C_interface(int bus) weak_function; diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp new file mode 100644 index 000000000..782ea62fe --- /dev/null +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file HMC5883_I2C.cpp + * + * I2C interface for HMC5883 / HMC 5983 + */ + +/* XXX trim includes */ +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <string.h> +#include <assert.h> +#include <debug.h> +#include <errno.h> +#include <unistd.h> + +#include <arch/board/board.h> + +#include <drivers/device/i2c.h> +#include <drivers/drv_mag.h> + +#include "hmc5883.h" + +#include "board_config.h" + +#ifdef PX4_I2C_OBDEV_HMC5883 + +#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 + +device::Device *HMC5883_I2C_interface(int bus); + +class HMC5883_I2C : public device::I2C +{ +public: + HMC5883_I2C(int bus); + virtual ~HMC5883_I2C(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +}; + +device::Device * +HMC5883_I2C_interface(int bus) +{ + return new HMC5883_I2C(bus); +} + +HMC5883_I2C::HMC5883_I2C(int bus) : + I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000) +{ +} + +HMC5883_I2C::~HMC5883_I2C() +{ +} + +int +HMC5883_I2C::init() +{ + /* this will call probe() */ + return I2C::init(); +} + +int +HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: + if (_bus == PX4_I2C_BUS_EXPANSION) { + return 1; + } else { + return 0; + } + + default: + ret = -EINVAL; + } + + return ret; +} + +int +HMC5883_I2C::probe() +{ + uint8_t data[3] = {0, 0, 0}; + + _retries = 10; + + if (read(ADDR_ID_A, &data[0], 1) || + read(ADDR_ID_B, &data[1], 1) || + read(ADDR_ID_C, &data[2], 1)) { + debug("read_reg fail"); + return -EIO; + } + + _retries = 2; + + if ((data[0] != ID_A_WHO_AM_I) || + (data[1] != ID_B_WHO_AM_I) || + (data[2] != ID_C_WHO_AM_I)) { + debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + return -EIO; + } + + return OK; +} + +int +HMC5883_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], count + 1, nullptr, 0); +} + +int +HMC5883_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +#endif /* PX4_I2C_OBDEV_HMC5883 */ diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp new file mode 100644 index 000000000..25a2f2b40 --- /dev/null +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file HMC5883_SPI.cpp + * + * SPI interface for HMC5983 + */ + +/* XXX trim includes */ +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <string.h> +#include <assert.h> +#include <debug.h> +#include <errno.h> +#include <unistd.h> + +#include <arch/board/board.h> + +#include <drivers/device/spi.h> +#include <drivers/drv_mag.h> + +#include "hmc5883.h" +#include <board_config.h> + +#ifdef PX4_SPIDEV_HMC + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#define HMC_MAX_SEND_LEN 4 +#define HMC_MAX_RCV_LEN 8 + +device::Device *HMC5883_SPI_interface(int bus); + +class HMC5883_SPI : public device::SPI +{ +public: + HMC5883_SPI(int bus, spi_dev_e device); + virtual ~HMC5883_SPI(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +}; + +device::Device * +HMC5883_SPI_interface(int bus) +{ + return new HMC5883_SPI(bus, (spi_dev_e)PX4_SPIDEV_HMC); +} + +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 */) +{ +} + +HMC5883_SPI::~HMC5883_SPI() +{ +} + +int +HMC5883_SPI::init() +{ + int ret; + + ret = SPI::init(); + if (ret != OK) { + debug("SPI init failed"); + return -EIO; + } + + // read WHO_AM_I value + uint8_t data[3] = {0, 0, 0}; + + if (read(ADDR_ID_A, &data[0], 1) || + read(ADDR_ID_B, &data[1], 1) || + read(ADDR_ID_C, &data[2], 1)) { + debug("read_reg fail"); + } + + if ((data[0] != ID_A_WHO_AM_I) || + (data[1] != ID_B_WHO_AM_I) || + (data[2] != ID_C_WHO_AM_I)) { + debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + return -EIO; + } + + return OK; +} + +int +HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: + +#ifdef PX4_SPI_BUS_EXT + if (_bus == PX4_SPI_BUS_EXT) { + return 1; + } else +#endif + { + return 0; + } + + default: + { + ret = -EINVAL; + } + } + + return ret; +} + +int +HMC5883_SPI::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_WRITE; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], &buf[0], count + 1); +} + +int +HMC5883_SPI::read(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_READ | ADDR_INCREMENT; + + int ret = transfer(&buf[0], &buf[0], count + 1); + memcpy(data, &buf[1], count); + return ret; +} + +#endif /* PX4_SPIDEV_HMC */ diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index be2ee7276..d4028b511 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,7 +37,7 @@ MODULE_COMMAND = hmc5883 -SRCS = hmc5883.cpp +SRCS = hmc5883_i2c.cpp hmc5883_spi.cpp hmc5883.cpp MODULE_STACKSIZE = 1200 diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 4b8e0c0b0..1aade450b 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -157,7 +157,7 @@ hott_sensors_thread_main(int argc, char *argv[]) /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ const int uart = open_uart(device); if (uart < 0) { - errx(1, "Failed opening HoTT UART, exiting."); + errx(1, "Open fail, exiting."); thread_running = false; } diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 82fa5cc6e..08bc1fead 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 */ @@ -200,6 +201,12 @@ public: */ void print_info(); + // print register dump + void print_registers(); + + // trigger an error + void test_error(); + protected: virtual int probe(); @@ -225,6 +232,9 @@ private: 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 +245,14 @@ private: 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 8 + 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. */ @@ -267,6 +285,11 @@ private: 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. */ void measure(); @@ -299,6 +322,14 @@ private: 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. * * @param max_dps The measurement range is set to permit reading at least @@ -338,6 +369,19 @@ private: 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, + 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 */), _call{}, @@ -355,11 +399,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 +436,7 @@ L3GD20::~L3GD20() perf_free(_sample_perf); perf_free(_reschedules); perf_free(_errors); + perf_free(_bad_registers); } int @@ -448,29 +496,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; } @@ -673,6 +719,18 @@ L3GD20::write_reg(unsigned reg, uint8_t value) } void +L3GD20::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + } + } +} + + +void L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { uint8_t val; @@ -680,7 +738,7 @@ L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) val = read_reg(reg); val &= ~clearbits; val |= setbits; - write_reg(reg, val); + write_checked_reg(reg, val); } int @@ -714,7 +772,7 @@ L3GD20::set_range(unsigned max_dps) _gyro_range_rad_s = new_range / 180.0f * M_PI_F; _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; - write_reg(ADDR_CTRL_REG4, bits); + write_checked_reg(ADDR_CTRL_REG4, bits); return OK; } @@ -750,7 +808,7 @@ L3GD20::set_samplerate(unsigned frequency) return -EINVAL; } - write_reg(ADDR_CTRL_REG1, bits); + write_checked_reg(ADDR_CTRL_REG1, bits); return OK; } @@ -791,6 +849,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; } } @@ -804,18 +867,18 @@ L3GD20::reset() disable_i2c(); /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); - write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ - write_reg(ADDR_CTRL_REG4, REG4_BDU); - write_reg(ADDR_CTRL_REG5, 0); - - write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + write_checked_reg(ADDR_CTRL_REG1, + REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ + write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ + write_checked_reg(ADDR_CTRL_REG4, REG4_BDU); + write_checked_reg(ADDR_CTRL_REG5, 0); + write_checked_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ /* disable FIFO. This makes things simpler and ensures we * aren't getting stale data. It means we must run the hrt * callback fast enough to not miss data. */ - write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); + write_checked_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); set_samplerate(0); // 760Hz or 800Hz set_range(L3GD20_DEFAULT_RANGE_DPS); @@ -840,19 +903,35 @@ L3GD20::measure_trampoline(void *arg) #endif void -L3GD20::measure() +L3GD20::check_registers(void) { -#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 + 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]); + } + _register_wait = 20; + } + _checked_next = (_checked_next+1) % L3GD20_NUM_CHECKED_REGISTERS; +} +void +L3GD20::measure() +{ /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -870,6 +949,20 @@ L3GD20::measure() /* start the performance counter */ perf_begin(_sample_perf); + 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; @@ -900,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) { @@ -973,7 +1066,39 @@ L3GD20::print_info() perf_print_counter(_sample_perf); perf_print_counter(_reschedules); perf_print_counter(_errors); + perf_print_counter(_bad_registers); _reports->print_info("report queue"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + } +} + +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"); +} + +void +L3GD20::test_error() +{ + // trigger a deliberate error + write_reg(ADDR_CTRL_REG3, 0); } int @@ -1011,6 +1136,8 @@ void start(bool external_bus, enum Rotation rotation); void test(); void reset(); void info(); +void regdump(); +void test_error(); /** * Start the driver. @@ -1149,10 +1276,40 @@ 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); +} + +/** + * 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'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -1209,5 +1366,17 @@ 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(); + + /* + * trigger an error + */ + if (!strcmp(verb, "testerror")) + l3gd20::test_error(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 4676c6ad7..e68f24152 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -746,6 +746,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; + } } } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 01c89192a..57754e4c0 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) @@ -242,14 +238,9 @@ public: void print_registers(); /** - * toggle logging - */ - void toggle_logging(); - - /** - * check for extreme accel values + * deliberately trigger an error */ - void check_extremes(const accel_report *arb); + void test_error(); protected: virtual int probe(); @@ -292,30 +283,30 @@ private: 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; + perf_counter_t _bad_values; + + 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; + // 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 +#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; + /** * Start automatic measurement. */ @@ -357,6 +348,11 @@ private: 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. */ void measure(); @@ -408,6 +404,14 @@ private: 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. * * @param max_g The measurement range of the accel is in g (9.81m/s^2) @@ -468,6 +472,19 @@ private: 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_REG3, + ADDR_CTRL_REG4, + ADDR_CTRL_REG5, + ADDR_CTRL_REG6, + ADDR_CTRL_REG7 }; + /** * Helper class implementing the mag driver node. */ @@ -528,23 +545,16 @@ 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")), + _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), - _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), + _constant_accel_count(0), + _checked_next(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; @@ -586,9 +596,8 @@ 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(_bad_values); perf_free(_accel_reschedules); } @@ -703,15 +712,14 @@ 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_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_REG7, REG7_CONT_MODE_M); + write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + 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); @@ -739,126 +747,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<sizeof(reglist); i++) { - regval[i] = read_reg(reglist[i]); - } - - // log registers at 10Hz when we have extreme values, or 0.5 Hz without - if (_last_log_reg_us == 0 || - (is_extreme && (now - _last_log_reg_us > 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<sizeof(reglist); i++) { - ::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]); - } - ::dprintf(_accel_log_fd, "\n"); - } - - // fsync at 0.1Hz - if (now - _last_log_sync_us > 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 +773,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++; } @@ -1263,6 +1154,17 @@ LSM303D::write_reg(unsigned reg, uint8_t value) } void +LSM303D::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + } + } +} + +void LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { uint8_t val; @@ -1270,7 +1172,7 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) val = read_reg(reg); val &= ~clearbits; val |= setbits; - write_reg(reg, val); + write_checked_reg(reg, val); } int @@ -1439,7 +1341,6 @@ LSM303D::accel_set_samplerate(unsigned frequency) } modify_reg(ADDR_CTRL_REG1, clearbits, setbits); - _reg1_expected = (_reg1_expected & ~clearbits) | setbits; return OK; } @@ -1515,24 +1416,35 @@ LSM303D::mag_measure_trampoline(void *arg) } void -LSM303D::measure() +LSM303D::check_registers(void) { - // 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 - if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { - perf_count(_accel_reschedules); - hrt_call_delay(&_accel_call, 100); - return; - } - if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { - perf_count(_reg1_resets); - reset(); - return; - } + 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]); + } + _register_wait = 20; + } + _checked_next = (_checked_next+1) % LSM303D_NUM_CHECKED_REGISTERS; +} +void +LSM303D::measure() +{ /* status register and data as read back from the device */ #pragma pack(push, 1) @@ -1550,6 +1462,30 @@ LSM303D::measure() /* start the performance counter */ perf_begin(_accel_sample_perf); + 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. + _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; @@ -1572,7 +1508,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 + // 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; @@ -1582,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); @@ -1611,12 +1582,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 { @@ -1691,8 +1656,22 @@ 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(_bad_values); + perf_print_counter(_accel_reschedules); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + } } void @@ -1750,20 +1729,13 @@ LSM303D::print_registers() for (uint8_t i=0; i<sizeof(regmap)/sizeof(regmap[0]); i++) { printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name); } - printf("_reg1_expected=0x%02x\n", _reg1_expected); - printf("_reg7_expected=0x%02x\n", _reg7_expected); } void -LSM303D::toggle_logging() +LSM303D::test_error() { - if (! _accel_logging_enabled) { - _accel_logging_enabled = true; - printf("Started logging to %s\n", ACCEL_LOGFILE); - } else { - _accel_logging_enabled = false; - printf("Stopped logging\n"); - } + // trigger an error + write_reg(ADDR_CTRL_REG3, 0); } LSM303D_mag::LSM303D_mag(LSM303D *parent) : @@ -1839,8 +1811,8 @@ void test(); void reset(); void info(); void regdump(); -void logging(); void usage(); +void test_error(); /** * Start the driver. @@ -2047,15 +2019,15 @@ regdump() } /** - * toggle logging + * trigger an error */ void -logging() +test_error() { if (g_dev == nullptr) errx(1, "driver not running\n"); - g_dev->toggle_logging(); + g_dev->test_error(); exit(0); } @@ -2063,7 +2035,7 @@ logging() void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2127,10 +2099,10 @@ lsm303d_main(int argc, char *argv[]) lsm303d::regdump(); /* - * dump device registers + * trigger an error */ - if (!strcmp(verb, "logging")) - lsm303d::logging(); + if (!strcmp(verb, "testerror")) + lsm303d::test_error(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index c9c27892f..6cac28a7d 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,16 @@ public: void print_registers(); + /** + * Test behaviour against factory offsets + * + * @return 0 on success, 1 on failure + */ + int factory_self_test(); + + // deliberately cause a sensor error + void test_error(); + protected: virtual int probe(); @@ -231,7 +248,12 @@ private: 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; + + uint8_t _register_wait; + uint64_t _reset_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -242,6 +264,18 @@ private: 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 9 + static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; + 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. */ @@ -257,7 +291,7 @@ private: * * 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 @@ -281,7 +315,7 @@ private: * @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); /** @@ -304,6 +338,14 @@ private: 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. * * @param max_g The maximum G value the range must support. @@ -347,11 +389,50 @@ private: */ 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&); + +#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) }; +/* + 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_PWR_MGMT_1, + MPUREG_USER_CTRL, + MPUREG_SMPLRT_DIV, + MPUREG_CONFIG, + MPUREG_GYRO_CONFIG, + MPUREG_ACCEL_CONFIG, + MPUREG_INT_ENABLE, + MPUREG_INT_PIN_CFG }; + + + /** * Helper class implementing the gyro driver node. */ @@ -407,14 +488,20 @@ 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")), + _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), _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), + _in_factory_test(false) { // disable debug() calls _debug_enabled = false; @@ -460,6 +547,7 @@ MPU6000::~MPU6000() perf_free(_accel_reads); perf_free(_gyro_reads); perf_free(_bad_transfers); + perf_free(_bad_registers); perf_free(_good_transfers); } @@ -486,7 +574,8 @@ MPU6000::init() if (_gyro_reports == nullptr) goto out; - reset(); + if (reset() != OK) + goto out; /* Initialize offsets and scales */ _accel_scale.x_offset = 0; @@ -571,27 +660,39 @@ out: 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_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); @@ -605,7 +706,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 +725,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 +740,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; } @@ -651,15 +752,16 @@ void 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 // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); + return OK; } int @@ -684,6 +786,7 @@ MPU6000::probe() case MPU6000_REV_D9: case MPU6000_REV_D10: debug("ID 0x%02x", _product); + _checked_values[0] = _product; return OK; } @@ -700,7 +803,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; } @@ -734,7 +837,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 @@ -833,6 +936,173 @@ 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 (as per datasheet) + */ +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<repeats; i++) { + up_udelay(1000); + mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)); + + accel_baseline[0] += int16_t_from_bytes(mpu_report.accel_x); + accel_baseline[1] += int16_t_from_bytes(mpu_report.accel_y); + accel_baseline[2] += int16_t_from_bytes(mpu_report.accel_z); + gyro_baseline[0] += int16_t_from_bytes(mpu_report.gyro_x); + gyro_baseline[1] += int16_t_from_bytes(mpu_report.gyro_y); + gyro_baseline[2] += int16_t_from_bytes(mpu_report.gyro_z); + } + +#if 1 + write_reg(MPUREG_GYRO_CONFIG, + BITS_FS_250DPS | + BITS_GYRO_ST_X | + BITS_GYRO_ST_Y | + BITS_GYRO_ST_Z); + + // accel 8g, self-test enabled all axes + write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config | 0xE0); +#endif + + up_udelay(20000); + + // get values with self-test enabled + set_frequency(MPU6000_HIGH_BUS_SPEED); + + + for (uint8_t i=0; i<repeats; i++) { + up_udelay(1000); + mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)); + accel[0] += int16_t_from_bytes(mpu_report.accel_x); + accel[1] += int16_t_from_bytes(mpu_report.accel_y); + accel[2] += int16_t_from_bytes(mpu_report.accel_z); + gyro[0] += int16_t_from_bytes(mpu_report.gyro_x); + gyro[1] += int16_t_from_bytes(mpu_report.gyro_y); + gyro[2] += int16_t_from_bytes(mpu_report.gyro_z); + } + + for (uint8_t i=0; i<3; i++) { + accel_baseline[i] /= repeats; + gyro_baseline[i] /= repeats; + accel[i] /= repeats; + gyro[i] /= repeats; + } + + // extract factory trim values + uint8_t trims[4]; + trims[0] = read_reg(MPUREG_TRIM1); + trims[1] = read_reg(MPUREG_TRIM2); + trims[2] = read_reg(MPUREG_TRIM3); + trims[3] = read_reg(MPUREG_TRIM4); + uint8_t atrim[3]; + uint8_t gtrim[3]; + + atrim[0] = ((trims[0]>>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; + 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) { @@ -874,8 +1144,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCRESET: - reset(); - return OK; + return reset(); case SENSORIOCSPOLLRATE: { switch (arg) { @@ -1094,12 +1363,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 +1413,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; i<MPU6000_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + } + } +} + int MPU6000::set_range(unsigned max_g) { @@ -1216,26 +1496,71 @@ MPU6000::measure_trampoline(void *arg) } 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. + */ + if (_register_wait == 0 || _checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + // after doing a reset we need to wait a long + // time before we do any other register writes + // or we will end up with the 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; + _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) % MPU6000_NUM_CHECKED_REGISTERS; +} + +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; + } + + 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; @@ -1254,6 +1579,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 +1619,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 +1656,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 +1751,21 @@ 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); + perf_print_counter(_reset_retries); _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<MPU6000_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED); + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + } } void @@ -1497,6 +1849,8 @@ void test(bool); void reset(bool); void info(bool); void regdump(bool); +void testerror(bool); +void factorytest(bool); void usage(); /** @@ -1688,10 +2042,40 @@ 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 + */ +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() { - 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"); @@ -1754,5 +2138,11 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) mpu6000::regdump(external_bus); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + if (!strcmp(verb, "factorytest")) + mpu6000::factorytest(external_bus); + + if (!strcmp(verb, "testerror")) + mpu6000::testerror(external_bus); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'"); } diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 889643d0d..0a793cbc9 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -91,12 +91,13 @@ static const int ERROR = -1; /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -#define MS5611_BARO_DEVICE_PATH "/dev/ms5611" +#define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" +#define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" class MS5611 : public device::CDev { public: - MS5611(device::Device *interface, ms5611::prom_u &prom_buf); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path); ~MS5611(); virtual int init(); @@ -133,6 +134,7 @@ protected: unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; + orb_id_t _orb_id; int _class_instance; @@ -195,8 +197,8 @@ protected: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); -MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : - CDev("MS5611", MS5611_BARO_DEVICE_PATH), +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) : + CDev("MS5611", path), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), @@ -224,7 +226,7 @@ MS5611::~MS5611() stop_cycle(); if (_class_instance != -1) - unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance); + unregister_class_devname(get_devname(), _class_instance); /* free any existing reports */ if (_reports != nullptr) @@ -261,6 +263,7 @@ MS5611::init() /* register alternate interfaces if we have to */ _class_instance = register_class_devname(BARO_DEVICE_PATH); + _orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance); struct baro_report brp; /* do a first measurement cycle to populate reports with valid data */ @@ -300,14 +303,7 @@ MS5611::init() ret = OK; - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - _baro_topic = orb_advertise(ORB_ID(sensor_baro0), &brp); - break; - case CLASS_DEVICE_SECONDARY: - _baro_topic = orb_advertise(ORB_ID(sensor_baro1), &brp); - break; - } + _baro_topic = orb_advertise(_orb_id, &brp); if (_baro_topic < 0) { warnx("failed to create sensor_baro publication"); @@ -729,15 +725,7 @@ MS5611::collect() /* publish it */ if (!(_pub_blocked)) { /* publish it */ - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - orb_publish(ORB_ID(sensor_baro0), _baro_topic, &report); - break; - - case CLASS_DEVICE_SECONDARY: - orb_publish(ORB_ID(sensor_baro1), _baro_topic, &report); - break; - } + orb_publish(_orb_id, _baro_topic, &report); } if (_reports->force(&report)) { @@ -787,13 +775,15 @@ MS5611::print_info() namespace ms5611 { -MS5611 *g_dev; +/* initialize explicitely for clarity */ +MS5611 *g_dev_ext = nullptr; +MS5611 *g_dev_int = nullptr; void start(bool external_bus); -void test(); -void reset(); +void test(bool external_bus); +void reset(bool external_bus); void info(); -void calibrate(unsigned altitude); +void calibrate(unsigned altitude, bool external_bus); void usage(); /** @@ -852,9 +842,13 @@ start(bool external_bus) int fd; prom_u prom_buf; - if (g_dev != nullptr) + 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, "already started"); + errx(0, "int already started"); + } device::Device *interface = nullptr; @@ -872,16 +866,35 @@ start(bool external_bus) errx(1, "interface init failed"); } - g_dev = new MS5611(interface, prom_buf); - if (g_dev == nullptr) { - delete interface; - errx(1, "failed to allocate driver"); + 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); + } - if (g_dev->init() != OK) - goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) { warnx("can't open baro device"); goto fail; @@ -895,9 +908,14 @@ start(bool external_bus) fail: - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (g_dev_int != nullptr) { + delete g_dev_int; + g_dev_int = nullptr; + } + + if (g_dev_ext != nullptr) { + delete g_dev_ext; + g_dev_ext = nullptr; } errx(1, "driver start failed"); @@ -909,16 +927,22 @@ fail: * and automatic modes. */ void -test() +test(bool external_bus) { struct baro_report report; ssize_t sz; int ret; - int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + int fd; + + if (external_bus) { + fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); + } else { + fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); + } if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); + err(1, "open failed (try 'ms5611 start' if the driver is not running)"); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -972,9 +996,15 @@ test() * Reset the driver. */ void -reset() +reset(bool external_bus) { - int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + int fd; + + if (external_bus) { + fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); + } else { + fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); + } if (fd < 0) err(1, "failed "); @@ -994,11 +1024,18 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev_ext == nullptr && g_dev_int == nullptr) errx(1, "driver not running"); - printf("state @ %p\n", g_dev); - g_dev->print_info(); + if (g_dev_ext) { + warnx("ext:"); + g_dev_ext->print_info(); + } + + if (g_dev_int) { + warnx("int:"); + g_dev_int->print_info(); + } exit(0); } @@ -1007,16 +1044,22 @@ info() * Calculate actual MSL pressure given current altitude */ void -calibrate(unsigned altitude) +calibrate(unsigned altitude, bool external_bus) { struct baro_report report; float pressure; float p1; - int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + int fd; + + if (external_bus) { + fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); + } else { + fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); + } if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); + err(1, "open failed (try 'ms5611 start' if the driver is not running)"); /* start the sensor polling at max */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) @@ -1101,6 +1144,12 @@ 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. */ @@ -1111,13 +1160,13 @@ ms5611_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(verb, "test")) - ms5611::test(); + ms5611::test(external_bus); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - ms5611::reset(); + ms5611::reset(external_bus); /* * Print driver information. @@ -1134,7 +1183,7 @@ ms5611_main(int argc, char *argv[]) long altitude = strtol(argv[optind+1], nullptr, 10); - ms5611::calibrate(altitude); + ms5611::calibrate(altitude, external_bus); } errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 62308fc65..9c9c1b0f8 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -62,6 +62,8 @@ #include <systemlib/perf_counter.h> #include <systemlib/err.h> +#include <conversion/rotation.h> + #include <drivers/drv_hrt.h> #include <drivers/drv_px4flow.h> #include <drivers/device/ringbuffer.h> @@ -73,13 +75,14 @@ #include <board_config.h> /* Configuration Constants */ -#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 -//range 0x42 - 0x49 +#define I2C_FLOW_ADDRESS 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49 /* PX4FLOW Registers addresses */ -#define PX4FLOW_REG 0x16 /* Measure Register 22*/ +#define PX4FLOW_REG 0x16 ///< Measure Register 22 + +#define PX4FLOW_CONVERSION_INTERVAL 20000 ///< in microseconds! 20000 = 50 Hz 100000 = 10Hz +#define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed -#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -125,7 +128,7 @@ struct i2c_integral_frame f_integral; class PX4FLOW: public device::I2C { public: - PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS); + PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS, enum Rotation rotation = (enum Rotation)0); virtual ~PX4FLOW(); virtual int init(); @@ -154,6 +157,8 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + + enum Rotation _sensor_rotation; /** * Test whether the device supported by the driver is present at a @@ -199,8 +204,8 @@ private: */ extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); -PX4FLOW::PX4FLOW(int bus, int address) : - I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz +PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, PX4FLOW_I2C_MAX_BUS_SPEED), /* 100-400 KHz */ _reports(nullptr), _sensor_ok(false), _measure_ticks(0), @@ -208,7 +213,8 @@ PX4FLOW::PX4FLOW(int bus, int address) : _px4flow_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")), + _sensor_rotation(rotation) { // enable debug() calls _debug_enabled = false; @@ -361,6 +367,13 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGQUEUEDEPTH: return _reports->size(); + case SENSORIOCSROTATION: + _sensor_rotation = (enum Rotation)arg; + return OK; + + case SENSORIOCGROTATION: + return _sensor_rotation; + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; @@ -535,6 +548,10 @@ PX4FLOW::collect() report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; + + /* rotate measurements according to parameter */ + float zeroval = 0.0f; + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); |