diff --git a/platforms/common/i2c_spi_buses.cpp b/platforms/common/i2c_spi_buses.cpp index ed82029ff9..c2c675462b 100644 --- a/platforms/common/i2c_spi_buses.cpp +++ b/platforms/common/i2c_spi_buses.cpp @@ -41,11 +41,146 @@ #include #include #include +#include + +const char *BusCLIArguments::parseDefaultArguments(int argc, char *argv[]) +{ + if (getopt(argc, argv, "") == EOF) { + return optarg(); + } + + // unexpected arguments + return nullptr; +} + +int BusCLIArguments::getopt(int argc, char *argv[], const char *options) +{ + if (_options[0] == 0) { // need to initialize + char *p = (char *)&_options; + + if (_i2c_support) { + *(p++) = 'X'; // external + *(p++) = 'I'; // internal + + if (i2c_address != 0) { + *(p++) = 'a'; *(p++) = ':'; // I2C address + } + } + + if (_spi_support) { + *(p++) = 'S'; // external + *(p++) = 's'; // internal + *(p++) = 'c'; *(p++) = ':'; // chip-select + *(p++) = 'm'; *(p++) = ':'; // spi mode + } + + *(p++) = 'b'; *(p++) = ':'; // bus + *(p++) = 'f'; *(p++) = ':'; // frequency + + // copy all options + const char *option = options; + + while (p != _options + sizeof(_options) && *option) { + if (*option != ':') { + // check for duplicates + for (const char *c = _options; c != p; ++c) { + if (*c == *option) { + PX4_ERR("conflicting option: %c", *c); + _options[0] = 0; + return EOF; + } + } + } + + *(p++) = *(option++); + } + + if (p == _options + sizeof(_options)) { + PX4_ERR("too many options"); + _options[0] = 0; + return EOF; + } + + *p = '\0'; + } + + int ch; + + while ((ch = px4_getopt(argc, argv, _options, &_optind, &_optarg)) != EOF) { + switch (ch) { + case 'X': + bus_option = I2CSPIBusOption::I2CExternal; + break; + + case 'I': + bus_option = I2CSPIBusOption::I2CInternal; + break; + + case 'a': + if (i2c_address == 0) { + return ch; + } + + i2c_address = (int)strtol(_optarg, nullptr, 0); + break; + + case 'S': + bus_option = I2CSPIBusOption::SPIExternal; + break; + + case 's': + bus_option = I2CSPIBusOption::SPIInternal; + break; + + case 'c': + chipselect_index = atoi(_optarg); + break; + + case 'b': + requested_bus = atoi(_optarg); + break; + + case 'f': + bus_frequency = 1000 * atoi(_optarg); + break; + + case 'm': + spi_mode = (spi_mode_e)atoi(_optarg); + break; + + default: + if (ch == '?') { + // abort further parsing on unknown arguments + _optarg = nullptr; + return EOF; + } + + return ch; + } + } + + if (ch == EOF) { + _optarg = argv[_optind]; + + // apply defaults if not provided + if (bus_frequency == 0) { + if (bus_option == I2CSPIBusOption::I2CExternal || bus_option == I2CSPIBusOption::I2CInternal) { + bus_frequency = default_i2c_frequency; + + } else if (bus_option == I2CSPIBusOption::SPIExternal || bus_option == I2CSPIBusOption::SPIInternal) { + bus_frequency = default_spi_frequency; + } + } + } + + return ch; +} + BusInstanceIterator::BusInstanceIterator(I2CSPIInstance **instances, int max_num_instances, const BusCLIArguments &cli_arguments, uint16_t devid_driver_index) : _instances(instances), _max_num_instances(max_num_instances), - _bus_option(cli_arguments.bus_option), _type(cli_arguments.type), + _bus_option(cli_arguments.bus_option), _type(cli_arguments.type), _i2c_address(cli_arguments.i2c_address), _spi_bus_iterator(spiFilter(cli_arguments.bus_option), cli_arguments.bus_option == I2CSPIBusOption::SPIExternal ? cli_arguments.chipselect_index : devid_driver_index, cli_arguments.requested_bus), @@ -83,7 +218,10 @@ bool BusInstanceIterator::next() continue; } - if (_bus_option == _instances[i]->_bus_option && bus == _instances[i]->_bus && _type == _instances[i]->_type) { + bool is_i2c = busType() == BOARD_I2C_BUS; + + if (_bus_option == _instances[i]->_bus_option && bus == _instances[i]->_bus && _type == _instances[i]->_type + && (!is_i2c || _i2c_address == _instances[i]->_i2c_address)) { _current_instance = i; } } diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index 7535ff203c..d8c2077dcf 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -43,6 +43,7 @@ #include #include #include +#include enum class I2CSPIBusOption : uint8_t { All = 0, ///< select all runnning instances @@ -59,31 +60,68 @@ enum class I2CSPIBusOption : uint8_t { class I2CSPIInstance { public: - I2CSPIInstance(I2CSPIBusOption bus_option, int bus, int type) - : _bus_option(bus_option), _bus(bus), _type(type) {} - virtual ~I2CSPIInstance() = default; private: + I2CSPIInstance(I2CSPIBusOption bus_option, int bus, uint8_t i2c_address, uint16_t type) + : _bus_option(bus_option), _bus(bus), _type(type), _i2c_address(i2c_address) {} + friend class BusInstanceIterator; friend class I2CSPIDriverBase; const I2CSPIBusOption _bus_option; const int _bus; - const int _type; ///< device type (driver-specific) + const int16_t _type; ///< device type (driver-specific) + const int8_t _i2c_address; ///< I2C address (optional) }; -struct BusCLIArguments { +class BusCLIArguments +{ +public: + BusCLIArguments(bool i2c_support, bool spi_support) + : _i2c_support(i2c_support), _spi_support(spi_support) {} + + /** + * Parse CLI arguments (for drivers that don't need any custom arguments, otherwise getopt() should be used) + * @return command (e.g. "start") or nullptr on error or unknown argument + */ + const char *parseDefaultArguments(int argc, char *argv[]); + + /** + * Like px4_getopt(), but adds and handles i2c/spi driver-specific arguments + */ + int getopt(int argc, char *argv[], const char *options); + + /** + * returns the current optional argument (for options like 'T:'), or the command (e.g. "start") + * @return nullptr or argument/command + */ + const char *optarg() const { return _optarg; } + + I2CSPIBusOption bus_option{I2CSPIBusOption::All}; - int type{0}; ///< device type (driver-specific) + uint16_t type{0}; ///< device type (driver-specific) int requested_bus{-1}; int chipselect_index{1}; Rotation rotation{ROTATION_NONE}; int bus_frequency{0}; - int spi_mode{-1}; + spi_mode_e spi_mode{SPIDEV_MODE3}; + uint8_t i2c_address{0}; ///< optional I2C address: a driver can set this to allow configuring the I2C address + int custom1{0}; ///< driver-specific custom argument int custom2{0}; ///< driver-specific custom argument void *custom_data{nullptr}; ///< driver-specific custom argument + + // driver defaults, if not specified via CLI + int default_spi_frequency{20 * 1000 * 1000}; ///< default spi bus frequency [Hz] + int default_i2c_frequency{400 * 1000}; ///< default i2c bus frequency [Hz] + +private: + char _options[32] {}; + int _optind{1}; + const char *_optarg{nullptr}; + const bool _i2c_support; + const bool _spi_support; }; /** @@ -117,7 +155,8 @@ private: I2CSPIInstance **_instances; const int _max_num_instances; const I2CSPIBusOption _bus_option; - const int _type; + const uint16_t _type; + const uint8_t _i2c_address; SPIBusIterator _spi_bus_iterator; I2CBusIterator _i2c_bus_iterator; int _current_instance{-1}; @@ -130,9 +169,10 @@ private: class I2CSPIDriverBase : public px4::ScheduledWorkItem, public I2CSPIInstance { public: - I2CSPIDriverBase(const char *module_name, const px4::wq_config_t &config, I2CSPIBusOption bus_option, int bus, int type) + I2CSPIDriverBase(const char *module_name, const px4::wq_config_t &config, I2CSPIBusOption bus_option, int bus, + uint8_t i2c_address, uint16_t type) : ScheduledWorkItem(module_name, config), - I2CSPIInstance(bus_option, bus, type) {} + I2CSPIInstance(bus_option, bus, i2c_address, type) {} static int module_stop(BusInstanceIterator &iterator); static int module_status(BusInstanceIterator &iterator); @@ -184,8 +224,9 @@ public: static I2CSPIInstance **instances() { return _instances; } protected: - I2CSPIDriver(const char *module_name, const px4::wq_config_t &config, I2CSPIBusOption bus_option, int bus, int type = 0) - : I2CSPIDriverBase(module_name, config, bus_option, bus, type) {} + I2CSPIDriver(const char *module_name, const px4::wq_config_t &config, I2CSPIBusOption bus_option, int bus, + uint8_t i2c_address = 0, uint16_t type = 0) + : I2CSPIDriverBase(module_name, config, bus_option, bus, i2c_address, type) {} virtual ~I2CSPIDriver() = default; diff --git a/src/lib/drivers/device/posix/SPI.hpp b/src/lib/drivers/device/posix/SPI.hpp index 8cfd7526cc..7220181897 100644 --- a/src/lib/drivers/device/posix/SPI.hpp +++ b/src/lib/drivers/device/posix/SPI.hpp @@ -186,6 +186,14 @@ protected: } // namespace device +#else + +enum spi_mode_e { + SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ + SPIDEV_MODE1 = 1, /* CPOL=0 CHPHA=1 */ + SPIDEV_MODE2 = 2, /* CPOL=1 CHPHA=0 */ + SPIDEV_MODE3 = 3 /* CPOL=1 CHPHA=1 */ +}; #endif // __PX4_LINUX #endif /* _DEVICE_SPI_H */