diff --git a/src/drivers/device/i2c.cpp b/src/drivers/device/i2c.cpp index 286778c014..33bb90fc97 100644 --- a/src/drivers/device/i2c.cpp +++ b/src/drivers/device/i2c.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,6 +45,8 @@ namespace device { +unsigned int I2C::_bus_clocks[3] = { 100000, 100000, 100000 }; + I2C::I2C(const char *name, const char *devname, int bus, @@ -76,12 +78,30 @@ I2C::~I2C() up_i2cuninitialize(_dev); } +int +I2C::set_bus_clock(unsigned bus, unsigned clock_hz) +{ + int index = bus - 1; + + if (index < 0 || index >= static_cast(sizeof(_bus_clocks) / sizeof(_bus_clocks[0]))) { + return -EINVAL; + } + + if (_bus_clocks[index] > 0) { + // debug("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz); + } + _bus_clocks[index] = clock_hz; + + return OK; +} + int I2C::init() { int ret = OK; + unsigned bus_index; - /* attach to the i2c bus */ + // attach to the i2c bus _dev = up_i2cinitialize(_bus); if (_dev == nullptr) { @@ -90,6 +110,37 @@ I2C::init() goto out; } + // the above call fails for a non-existing bus index, + // so the index math here is safe. + bus_index = _bus - 1; + + // abort if the max frequency we allow (the frequency we ask) + // is smaller than the bus frequency + if (_bus_clocks[bus_index] > _frequency) { + (void)up_i2cuninitialize(_dev); + log("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", + _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); + ret = -EINVAL; + goto out; + } + + // set the bus frequency on the first access if it has + // not been set yet + if (_bus_clocks[bus_index] == 0) { + _bus_clocks[bus_index] = _frequency; + } + + // set frequency for this instance once to the bus speed + // the bus speed is the maximum supported by all devices on the bus, + // as we have to prioritize performance over compatibility. + // If a new device requires a lower clock speed, this has to be + // manually set via "fmu i2c " before starting any + // drivers. + // This is necessary as automatically lowering the bus speed + // for maximum compatibility could induce timing issues on + // critical sensors the adopter might be unaware of. + I2C_SETFREQUENCY(_dev, _bus_clocks[bus_index]); + // call the probe function to check whether the device is present ret = probe(); @@ -107,9 +158,13 @@ I2C::init() } // tell the world where we are - log("on I2C bus %d at 0x%02x", _bus, _address); + log("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", + _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); out: + if ((ret != OK) && (_dev != nullptr)) { + up_i2cuninitialize(_dev); + } return ret; } @@ -152,12 +207,6 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re if (msgs == 0) return -EINVAL; - /* - * I2C architecture means there is an unavoidable race here - * if there are any devices on the bus with a different frequency - * preference. Really, this is pointless. - */ - I2C_SETFREQUENCY(_dev, _frequency); ret = I2C_TRANSFER(_dev, &msgv[0], msgs); /* success */ @@ -186,12 +235,6 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs) do { - /* - * I2C architecture means there is an unavoidable race here - * if there are any devices on the bus with a different frequency - * preference. Really, this is pointless. - */ - I2C_SETFREQUENCY(_dev, _frequency); ret = I2C_TRANSFER(_dev, msgv, msgs); /* success */ diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 8518596ea6..7bb4ae1aff 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -59,6 +59,10 @@ public: * Get the address */ int16_t get_address() const { return _address; } + + static int set_bus_clock(unsigned bus, unsigned clock_hz); + + static unsigned int _bus_clocks[3]; protected: /** diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index c001fb1e57..5acff70b5f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -55,6 +55,7 @@ #include #include +#include #include #include #include @@ -111,6 +112,8 @@ public: int set_pwm_alt_rate(unsigned rate); int set_pwm_alt_channels(uint32_t channels); + int set_i2c_bus_clock(unsigned bus, unsigned clock_hz); + private: #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) static const unsigned _max_actuators = 4; @@ -505,6 +508,12 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels) return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate); } +int +PX4FMU::set_i2c_bus_clock(unsigned bus, unsigned clock_hz) +{ + return device::I2C::set_bus_clock(bus, clock_hz); +} + void PX4FMU::subscribe() { @@ -1593,6 +1602,11 @@ fmu_new_mode(PortMode new_mode) return OK; } +int fmu_new_i2c_speed(unsigned bus, unsigned clock_hz) +{ + return g_fmu->set_i2c_bus_clock(bus, clock_hz); +} + int fmu_start(void) { @@ -1867,12 +1881,27 @@ fmu_main(int argc, char *argv[]) exit(0); } + if (!strcmp(verb, "i2c")) { + if (argc > 3) { + int bus = strtol(argv[2], 0, 0); + int clock_hz = strtol(argv[3], 0, 0); + int ret = fmu_new_i2c_speed(bus, clock_hz); + + if (ret) { + errx(ret, "setting I2C clock failed"); + } + + exit(0); + } else { + warnx("i2c cmd args: "); + } + } fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE) - fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n"); + fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds], i2c \n"); #endif exit(1); }