diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 626f6ab920..43061b08c0 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -47,8 +47,8 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); -BATT_SMBUS::BATT_SMBUS(SMBus *interface, const char *path) : - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), +BATT_SMBUS::BATT_SMBUS(I2CSPIBusOption bus_option, const int bus, SMBus *interface) : + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), _interface(interface) { battery_status_s new_report = {}; @@ -81,79 +81,8 @@ BATT_SMBUS::~BATT_SMBUS() param_set(param_find("BAT_SOURCE"), &battsource); } -int BATT_SMBUS::task_spawn(int argc, char *argv[]) +void BATT_SMBUS::RunImpl() { - enum BATT_SMBUS_BUS busid = BATT_SMBUS_BUS_ALL; - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "XTRIA", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'X': - busid = BATT_SMBUS_BUS_I2C_EXTERNAL; - break; - - case 'T': - busid = BATT_SMBUS_BUS_I2C_EXTERNAL1; - break; - - case 'R': - busid = BATT_SMBUS_BUS_I2C_EXTERNAL2; - break; - - case 'I': - busid = BATT_SMBUS_BUS_I2C_INTERNAL; - break; - - case 'A': - busid = BATT_SMBUS_BUS_ALL; - break; - - default: - print_usage(); - return PX4_ERROR; - } - } - - for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { - - if (!is_running() && (busid == BATT_SMBUS_BUS_ALL || smbus_bus_options[i].busid == busid)) { - - SMBus *interface = new SMBus(smbus_bus_options[i].busnum, BATT_SMBUS_ADDR); - BATT_SMBUS *dev = new BATT_SMBUS(interface, smbus_bus_options[i].devpath); - - int result = dev->get_startup_info(); - - if (result != PX4_OK) { - delete dev; - continue; - } - - // Successful read of device type, we've found our battery - _object.store(dev); - _task_id = task_id_is_work_queue; - - dev->ScheduleOnInterval(BATT_SMBUS_MEASUREMENT_INTERVAL_US); - - return PX4_OK; - - } - } - - PX4_WARN("Not found."); - return PX4_ERROR; -} - -void BATT_SMBUS::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - // Get the current time. uint64_t now = hrt_absolute_time(); @@ -354,7 +283,7 @@ int BATT_SMBUS::dataflash_read(uint16_t &address, void *data, const unsigned len return result; } -int BATT_SMBUS::dataflash_write(uint16_t &address, void *data, const unsigned length) +int BATT_SMBUS::dataflash_write(uint16_t address, void *data, const unsigned length) { uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS; @@ -387,7 +316,7 @@ int BATT_SMBUS::get_startup_info() result = manufacturer_name((uint8_t *)man_name, sizeof(man_name)); if (result != PX4_OK) { - PX4_WARN("Failed to get manufacturer name"); + PX4_DEBUG("Failed to get manufacturer name"); return PX4_ERROR; } @@ -559,96 +488,7 @@ int BATT_SMBUS::lifetime_read_block_one() return PX4_OK; } -int BATT_SMBUS::custom_command(int argc, char *argv[]) -{ - const char *input = argv[0]; - uint8_t man_name[22]; - int result = 0; - - if (!is_running()) { - PX4_ERR("not running"); - return -1; - } - - BATT_SMBUS *obj = get_instance(); - - if (!strcmp(input, "man_info")) { - - result = obj->manufacturer_name(man_name, sizeof(man_name)); - PX4_INFO("The manufacturer name: %s", man_name); - - result = obj->manufacture_date(); - PX4_INFO("The manufacturer date: %d", result); - - uint16_t serial_num = 0; - serial_num = obj->get_serial_number(); - PX4_INFO("The serial number: %d", serial_num); - - return 0; - } - - if (!strcmp(input, "unseal")) { - obj->unseal(); - return 0; - } - - if (!strcmp(input, "seal")) { - obj->seal(); - return 0; - } - - if (!strcmp(input, "suspend")) { - obj->suspend(); - return 0; - } - - if (!strcmp(input, "resume")) { - obj->resume(); - return 0; - } - - if (!strcmp(input, "serial_num")) { - uint16_t serial_num = obj->get_serial_number(); - PX4_INFO("Serial number: %d", serial_num); - return 0; - } - - if (!strcmp(input, "write_flash")) { - if (argc >= 3) { - uint16_t address = atoi(argv[1]); - unsigned length = atoi(argv[2]); - uint8_t tx_buf[32] = {}; - - if (length > 32) { - PX4_WARN("Data length out of range: Max 32 bytes"); - return 1; - } - - // Data needs to be fed in 1 byte (0x01) at a time. - for (unsigned i = 0; i < length; i++) { - if ((unsigned)argc <= 3 + i) { - tx_buf[i] = atoi(argv[3 + i]); - } - } - - if (PX4_OK != obj->dataflash_write(address, tx_buf, length)) { - PX4_INFO("Dataflash write failed: %d", address); - px4_usleep(100000); - return 1; - - } else { - px4_usleep(100000); - return 0; - } - } - } - - print_usage(); - - return PX4_ERROR; -} - -int BATT_SMBUS::print_usage() +void BATT_SMBUS::print_usage() { PRINT_MODULE_DESCRIPTION( R"DESCR_STR( @@ -664,11 +504,8 @@ $ batt_smbus -X write_flash 19069 2 27 0 PRINT_MODULE_USAGE_NAME("batt_smbus", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_FLAG('X', "BATT_SMBUS_BUS_I2C_EXTERNAL", true); - PRINT_MODULE_USAGE_PARAM_FLAG('T', "BATT_SMBUS_BUS_I2C_EXTERNAL1", true); - PRINT_MODULE_USAGE_PARAM_FLAG('R', "BATT_SMBUS_BUS_I2C_EXTERNAL2", true); - PRINT_MODULE_USAGE_PARAM_FLAG('I', "BATT_SMBUS_BUS_I2C_INTERNAL", true); - PRINT_MODULE_USAGE_PARAM_FLAG('A', "BATT_SMBUS_BUS_ALL", true); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x0B); PRINT_MODULE_USAGE_COMMAND_DESCR("man_info", "Prints manufacturer info."); PRINT_MODULE_USAGE_COMMAND_DESCR("unseal", "Unseals the devices flash memory to enable write_flash commands."); @@ -681,11 +518,151 @@ $ batt_smbus -X write_flash 19069 2 27 0 PRINT_MODULE_USAGE_ARG("number of bytes", "Number of bytes to send.", true); PRINT_MODULE_USAGE_ARG("data[0]...data[n]", "One byte of data at a time separated by spaces.", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return PX4_OK; } -int batt_smbus_main(int argc, char *argv[]) +I2CSPIDriverBase *BATT_SMBUS::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - return BATT_SMBUS::main(argc, argv); + SMBus *interface = new SMBus(iterator.bus(), cli.i2c_address); + if (interface == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + BATT_SMBUS *instance = new BATT_SMBUS(iterator.configuredBusOption(), iterator.bus(), interface); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + int result = instance->get_startup_info(); + + if (result != PX4_OK) { + delete instance; + return nullptr; + } + + instance->ScheduleOnInterval(BATT_SMBUS_MEASUREMENT_INTERVAL_US); + + return instance; +} + +void +BATT_SMBUS::custom_method(const BusCLIArguments &cli) +{ + switch(cli.custom1) { + case 1: { + uint8_t man_name[22]; + int result = manufacturer_name(man_name, sizeof(man_name)); + PX4_INFO("The manufacturer name: %s", man_name); + + result = manufacture_date(); + PX4_INFO("The manufacturer date: %d", result); + + uint16_t serial_num = 0; + serial_num = get_serial_number(); + PX4_INFO("The serial number: %d", serial_num); + } + break; + case 2: + unseal(); + break; + case 3: + seal(); + break; + case 4: + suspend(); + break; + case 5: + resume(); + break; + case 6: + if (cli.custom_data) { + unsigned address = cli.custom2; + uint8_t *tx_buf = (uint8_t*)cli.custom_data; + unsigned length = tx_buf[0]; + + if (PX4_OK != dataflash_write(address, tx_buf+1, length)) { + PX4_ERR("Dataflash write failed: %d", address); + } + px4_usleep(100000); + } + break; + } +} + +extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]) +{ + using ThisDriver = BATT_SMBUS; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 100000; + cli.i2c_address = BATT_SMBUS_ADDR; + + const char *verb = cli.parseDefaultArguments(argc, argv); + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BAT_DEVTYPE_SMBUS); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + if (!strcmp(verb, "man_info")) { + cli.custom1 = 1; + return ThisDriver::module_custom_method(cli, iterator, false); + } + if (!strcmp(verb, "unseal")) { + cli.custom1 = 2; + return ThisDriver::module_custom_method(cli, iterator); + } + if (!strcmp(verb, "seal")) { + cli.custom1 = 3; + return ThisDriver::module_custom_method(cli, iterator); + } + if (!strcmp(verb, "suspend")) { + cli.custom1 = 4; + return ThisDriver::module_custom_method(cli, iterator); + } + if (!strcmp(verb, "resume")) { + cli.custom1 = 5; + return ThisDriver::module_custom_method(cli, iterator); + } + if (!strcmp(verb, "write_flash")) { + cli.custom1 = 6; + if (argc >= 3) { + uint16_t address = atoi(argv[1]); + unsigned length = atoi(argv[2]); + uint8_t tx_buf[33]; + cli.custom_data = &tx_buf; + + if (length > 32) { + PX4_WARN("Data length out of range: Max 32 bytes"); + return 1; + } + + tx_buf[0] = length; + // Data needs to be fed in 1 byte (0x01) at a time. + for (unsigned i = 0; i < length; i++) { + if ((unsigned)argc <= 3 + i) { + tx_buf[i+1] = atoi(argv[3 + i]); + } + } + cli.custom2 = address; + return ThisDriver::module_custom_method(cli, iterator); + } + } + + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h index 5bbe04d327..622132fe8d 100644 --- a/src/drivers/batt_smbus/batt_smbus.h +++ b/src/drivers/batt_smbus/batt_smbus.h @@ -49,10 +49,10 @@ #include #include #include -#include +#include #include -#include "board_config.h" +#include #define MAC_DATA_BUFFER_SIZE 32 @@ -95,51 +95,22 @@ #define BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT 0xcf #define BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED 0xce -#define NUM_BUS_OPTIONS (sizeof(smbus_bus_options)/sizeof(smbus_bus_options[0])) - -enum BATT_SMBUS_BUS { - BATT_SMBUS_BUS_ALL = 0, - BATT_SMBUS_BUS_I2C_INTERNAL, - BATT_SMBUS_BUS_I2C_EXTERNAL, - BATT_SMBUS_BUS_I2C_EXTERNAL1, - BATT_SMBUS_BUS_I2C_EXTERNAL2 -}; - -struct batt_smbus_bus_option { - enum BATT_SMBUS_BUS busid; - const char *devpath; - uint8_t busnum; -} const smbus_bus_options[] = { - { BATT_SMBUS_BUS_I2C_EXTERNAL, "/dev/batt_smbus_ext", PX4_I2C_BUS_EXPANSION}, -#ifdef PX4_I2C_BUS_EXPANSION1 - { BATT_SMBUS_BUS_I2C_EXTERNAL1, "/dev/batt_smbus_ext1", PX4_I2C_BUS_EXPANSION1}, -#endif -#ifdef PX4_I2C_BUS_EXPANSION2 - { BATT_SMBUS_BUS_I2C_EXTERNAL2, "/dev/batt_smbus_ext2", PX4_I2C_BUS_EXPANSION2}, -#endif -#ifdef PX4_I2C_BUS_ONBOARD - { BATT_SMBUS_BUS_I2C_INTERNAL, "/dev/batt_smbus_int", PX4_I2C_BUS_ONBOARD}, -#endif -}; - -class BATT_SMBUS : public ModuleBase, public px4::ScheduledWorkItem +class BATT_SMBUS : public I2CSPIDriver { public: - - BATT_SMBUS(SMBus *interface, const char *path); + BATT_SMBUS(I2CSPIBusOption bus_option, const int bus, SMBus *interface); ~BATT_SMBUS(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + friend SMBus; - /** @see ModuleBase */ - static int print_usage(); + void RunImpl(); - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); + void custom_method(const BusCLIArguments &cli) override; /** * @brief Reads data from flash. @@ -156,7 +127,7 @@ public: * @param length The number of bytes being written. * @return Returns PX4_OK on success, PX4_ERROR on failure. */ - int dataflash_write(uint16_t &address, void *data, const unsigned length); + int dataflash_write(uint16_t address, void *data, const unsigned length); /** * @brief Returns the SBS serial number of the battery device. @@ -244,8 +215,6 @@ public: private: - void Run() override; - SMBus *_interface; perf_counter_t _cycle{perf_alloc(PC_ELAPSED, "batt_smbus_cycle")}; diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 9cd8bb391b..4dd6e3b607 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -141,6 +141,7 @@ #define DRV_LED_DEVTYPE_BLINKM 0x79 #define DRV_LED_DEVTYPE_RGBLED 0x7a #define DRV_LED_DEVTYPE_RGBLED_NCP5623C 0x7b +#define DRV_BAT_DEVTYPE_SMBUS 0x7c #define DRV_DEVTYPE_UNUSED 0xff