diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 53b9485f5f8..63e80a4993f 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -70,6 +70,10 @@ BATT_SMBUS::BATT_SMBUS(SMBus *interface, const char *path) : param_set(param_find("BAT_SOURCE"), &battsource); _interface->init(); + // unseal() here to allow an external config script to write to protected flash. + // This is neccessary to avoid bus errors due to using standard i2c mode instead of SMbus mode. + // The external config script should then seal() the device. + unseal(); } BATT_SMBUS::~BATT_SMBUS() @@ -211,10 +215,6 @@ void BATT_SMBUS::cycle() // Read remaining capacity. ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, &result); - if (result > _batt_capacity) { - _batt_capacity = result; - } - // Calculate remaining capacity percent with complementary filter. new_report.remaining = 0.8f * _last_report.remaining + 0.2f * (1.0f - (float)((float)(_batt_capacity - result) / (float)_batt_capacity)); @@ -267,12 +267,27 @@ void BATT_SMBUS::cycle() exit_and_cleanup(); } else { + + while (_should_suspend) { + usleep(200000); + } + // 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_US)); } } +void BATT_SMBUS::suspend() +{ + _should_suspend = true; +} + +void BATT_SMBUS::resume() +{ + _should_suspend = false; +} + int BATT_SMBUS::get_cell_voltages() { // Temporary variable for storing SMBUS reads. @@ -319,14 +334,13 @@ void BATT_SMBUS::set_undervoltage_protection(float average_current) uint8_t protections_a_tmp = BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED; uint16_t address = BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS; - unseal(); - - if (write_flash(address, &protections_a_tmp, 1) == PX4_OK) { + if (dataflash_write(address, &protections_a_tmp, 1) == PX4_OK) { _cell_undervoltage_protection_status = 0; PX4_WARN("Disabled CUV"); - } - seal(); + } else { + PX4_WARN("Failed to disable CUV"); + } } } else { @@ -336,15 +350,13 @@ void BATT_SMBUS::set_undervoltage_protection(float average_current) uint8_t protections_a_tmp = BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT; uint16_t address = BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS; - unseal(); - - if (write_flash(address, &protections_a_tmp, 1) == PX4_OK) { + if (dataflash_write(address, &protections_a_tmp, 1) == PX4_OK) { _cell_undervoltage_protection_status = 1; PX4_WARN("Enabled CUV"); + } else { + PX4_WARN("Failed to enable CUV"); } - - seal(); } } } @@ -357,7 +369,7 @@ int BATT_SMBUS::dataflash_read(uint16_t &address, void *data) uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS; // address is 2 bytes - int result = _interface->block_write(code, &address, 2, false); + int result = _interface->block_write(code, &address, 2, true); if (result != PX4_OK) { return result; @@ -431,14 +443,14 @@ int BATT_SMBUS::get_startup_info() } if (lifetime_data_flush() == PX4_OK) { + // Flush needs time to complete, otherwise device is busy. 100ms not enough, 200ms works. + usleep(200000); + if (lifetime_read_block_one() == PX4_OK) { if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) { PX4_WARN("Battery Damaged Will Not Fly. Lifetime max voltage difference: %4.2f", (double)_lifetime_max_delta_cell_voltage); } - - } else { - PX4_WARN("Failed to read lifetime block 1"); } } else { @@ -541,15 +553,19 @@ int BATT_SMBUS::manufacturer_write(const uint16_t cmd_code, void *data, const un int BATT_SMBUS::unseal() { - // See pg95 of bq40z50 technical reference. + // See bq40z50 technical reference. uint16_t keys[2] = {0x0414, 0x3672}; - return manufacturer_write(keys[0], &keys[1], 2); + int ret = _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, &keys[0]); + + ret |= _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, &keys[1]); + + return ret; } int BATT_SMBUS::seal() { - // See pg95 of bq40z50 technical reference. + // See bq40z50 technical reference. uint16_t reg = BATT_SMBUS_SEAL; return manufacturer_write(reg, nullptr, 0); @@ -557,7 +573,6 @@ int BATT_SMBUS::seal() int BATT_SMBUS::lifetime_data_flush() { - // See pg95 of bq40z50 technical reference. uint16_t flush = BATT_SMBUS_LIFETIME_FLUSH; return manufacturer_write(flush, nullptr, 0); @@ -581,25 +596,6 @@ int BATT_SMBUS::lifetime_read_block_one() return PX4_OK; } -int BATT_SMBUS::write_flash(uint16_t address, uint8_t *tx_buf, const unsigned length) -{ - - if (length > 32) { - PX4_WARN("Data length out of range: Max 32 bytes"); - return PX4_ERROR; - } - - if (PX4_OK != dataflash_write(address, tx_buf, length)) { - PX4_INFO("Dataflash write failed: %d", address); - usleep(100000); - return PX4_ERROR; - - } else { - usleep(100000); - return PX4_OK; - } -} - int BATT_SMBUS::custom_command(int argc, char *argv[]) { const char *input = argv[0]; @@ -638,6 +634,16 @@ int BATT_SMBUS::custom_command(int argc, char *argv[]) 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); @@ -657,7 +663,7 @@ int BATT_SMBUS::custom_command(int argc, char *argv[]) // Data needs to be fed in 1 byte (0x01) at a time. for (unsigned i = 0; i < length; i++) { - tx_buf[i] = atoi(argv[5 + i]); + tx_buf[i] = atoi(argv[3 + i]); } if (PX4_OK != obj->dataflash_write(address, tx_buf, length)) { @@ -703,6 +709,8 @@ $ batt_smbus -X write_flash 19069 2 27 0 PRINT_MODULE_USAGE_COMMAND_DESCR("report", "Prints the last report."); PRINT_MODULE_USAGE_COMMAND_DESCR("unseal", "Unseals the devices flash memory to enable write_flash commands."); PRINT_MODULE_USAGE_COMMAND_DESCR("seal", "Seals the devices flash memory to disbale write_flash commands."); + PRINT_MODULE_USAGE_COMMAND_DESCR("suspend", "Suspends the driver from rescheduling the cycle."); + PRINT_MODULE_USAGE_COMMAND_DESCR("resume", "Resumes the driver from suspension."); PRINT_MODULE_USAGE_COMMAND_DESCR("write_flash", "Writes to flash. The device must first be unsealed with the unseal command."); PRINT_MODULE_USAGE_ARG("address", "The address to start writing.", true); diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h index ac2c3a3cf71..36a857879f1 100644 --- a/src/drivers/batt_smbus/batt_smbus.h +++ b/src/drivers/batt_smbus/batt_smbus.h @@ -261,15 +261,6 @@ public: */ int lifetime_read_block_one(); - /** - * @brief Reads the lifetime data from block 1. - * @param address Address of the register to write - * @param tx_buf The sent data. - * @param length The number of bytes being written. - * @return Returns PX4_OK on success, PX4_ERROR on failure. - */ - int write_flash(uint16_t address, uint8_t *tx_buf, const unsigned length); - /** * @brief Reads the cell voltages. * @return Returns PX4_OK on success or associated read error code on failure. @@ -283,6 +274,10 @@ public: SMBus *_interface; + void suspend(); + + void resume(); + private: static void cycle_trampoline(void *arg); @@ -297,6 +292,8 @@ private: float _min_cell_voltage{0}; + bool _should_suspend{false}; + void cycle(); /** @param _last_report Last published report, used for test(). */ diff --git a/src/drivers/batt_smbus/batt_smbus_main.cpp b/src/drivers/batt_smbus/batt_smbus_main.cpp deleted file mode 100644 index 76378b42429..00000000000 --- a/src/drivers/batt_smbus/batt_smbus_main.cpp +++ /dev/null @@ -1,536 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 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. - * - ****************************************************************************/ - -#include "batt_smbus.h" -#include -#include - -extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); - -enum BATT_SMBUS_BUS { - BATT_SMBUS_BUS_ALL = 0, - BATT_SMBUS_BUS_I2C_INTERNAL, - BATT_SMBUS_BUS_I2C_EXTERNAL, -}; - -/** - * Local functions in support of the shell command. - */ - -namespace batt_smbus -{ - -struct batt_smbus_bus_option { - enum BATT_SMBUS_BUS busid; - const char *devpath; - BATT_SMBUS_constructor interface_constructor; - uint8_t busnum; - BATT_SMBUS *dev; -} bus_options[] = { -#ifdef PX4_I2C_BUS_ONBOARD - { BATT_SMBUS_BUS_I2C_INTERNAL, "/dev/batt_smbus_int", &BATT_SMBUS_I2C_interface, PX4_I2C_BUS_ONBOARD, nullptr }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION - { BATT_SMBUS_BUS_I2C_EXTERNAL, "/dev/batt_smbus_ext", &BATT_SMBUS_I2C_interface, PX4_I2C_BUS_EXPANSION, nullptr }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION1 - { BATT_SMBUS_BUS_I2C_EXTERNAL, "/dev/batt_smbus_ext1", &BATT_SMBUS_I2C_interface, PX4_I2C_BUS_EXPANSION1, nullptr }, -#endif -#ifdef PX4_I2C_BUS_EXPANSION2 - { BATT_SMBUS_BUS_I2C_EXTERNAL, "/dev/batt_smbus_ext2", &BATT_SMBUS_I2C_interface, PX4_I2C_BUS_EXPANSION2, nullptr }, -#endif -}; - -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) - -int start(enum BATT_SMBUS_BUS busid); -bool start_bus(struct batt_smbus_bus_option &bus); -struct batt_smbus_bus_option &find_bus(enum BATT_SMBUS_BUS busid); -int reset(enum BATT_SMBUS_BUS busid); -int info(); - -void usage(const char *reason); -int manufacture_date(); -int manufacturer_name(); -int serial_number(); - -/** - * start driver for a specific bus option - */ -bool start_bus(struct batt_smbus_bus_option &bus) -{ - PX4_INFO("starting %s", bus.devpath); - - if (bus.dev != nullptr) { - PX4_WARN("bus option already started"); - return false; - } - - device::Device *interface = bus.interface_constructor(bus.busnum); - - if (interface->init() != OK) { - delete interface; - PX4_WARN("no device on bus %u", (unsigned)bus.busid); - return false; - } - - bus.dev = new BATT_SMBUS(interface, bus.devpath); - - if (bus.dev != nullptr && PX4_OK != bus.dev->init()) { - PX4_WARN("init failed"); - delete bus.dev; - bus.dev = nullptr; - return false; - } - - return true; -} - -/** - * Start the driver. - * - * This function call only returns once the driver - * is either successfully up and running or failed to start. - */ -int start(enum BATT_SMBUS_BUS busid) -{ - for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { - - if (busid == BATT_SMBUS_BUS_ALL && bus_options[i].dev != nullptr) { - // This device is already started. - PX4_INFO("Smart battery %p already started", bus_options[i].dev); - continue; - } - - if (busid != BATT_SMBUS_BUS_ALL && bus_options[i].busid != busid) { - // Not the one that is asked for. - continue; - } - - if (start_bus(bus_options[i])) { - return PX4_OK; - } - - PX4_INFO("Smart battery failed to start"); - - if (busid != BATT_SMBUS_BUS_ALL) { - return PX4_ERROR; - } - - } - - return PX4_ERROR; -} - -/** - * Find a bus structure for a busid. - */ -struct batt_smbus_bus_option &find_bus(enum BATT_SMBUS_BUS busid) -{ - for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { - if ((busid == BATT_SMBUS_BUS_ALL || busid == bus_options[i].busid) && - bus_options[i].dev != nullptr) { - return bus_options[i]; - } - } - - errx(1, "Could not find a smart battery: Did you start it?"); - // to satisfy other compilers - return bus_options[0]; -} - - - -int manufacture_date() -{ - int result = -1; - uint16_t man_date = 0; - - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - struct batt_smbus_bus_option &bus = bus_options[i]; - - if (bus.dev != nullptr) { - PX4_INFO("%s", bus.devpath); - result = bus.dev->manufacture_date(&man_date); - } - } - - if (PX4_OK == result) { - // Convert the uint16_t into human-readable date format. - uint16_t year = ((man_date >> 9) & 0xFF) + 1980; - uint8_t month = (man_date >> 5) & 0xF; - uint8_t day = man_date & 0x1F; - PX4_INFO("The manufacturer date is: %4d-%02d-%02d", year, month, day); - return PX4_OK; - - } else { - PX4_WARN("Unable to read the manufacturer date."); - return PX4_ERROR; - } - -} - -int manufacturer_name() -{ - int result = -1; - uint8_t man_name[22]; - - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - struct batt_smbus_bus_option &bus = bus_options[i]; - - if (bus.dev != nullptr) { - PX4_INFO("%s", bus.devpath); - result = bus.dev->manufacturer_name(man_name, sizeof(man_name)); - } - } - - if (PX4_OK == result) { - PX4_INFO("The manufacturer name: %s", man_name); - return PX4_OK; - - } else { - PX4_WARN("Unable to read manufacturer name."); - return PX4_ERROR; - } -} - -int serial_number() -{ - uint16_t serial_num = 0; - - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - struct batt_smbus_bus_option &bus = bus_options[i]; - - if (bus.dev != nullptr) { - PX4_INFO("%s", bus.devpath); - serial_num = bus.dev->get_serial_number(); - } - } - - PX4_INFO("The serial number: 0x%04x (%d in decimal)", serial_num, serial_num); - - return PX4_OK; -} - -void usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -Smart battery driver for the BQ40Z50 fuel gauge IC. - -### Examples -To write to flash to set parameters. address, number_of_bytes, byte0, ... , byteN -$ batt_smbus -X write_flash 19069 2 27 0 - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("batt_smbus", "driver"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_STRING('X', "BATT_SMBUS_BUS_I2C_EXTERNAL", nullptr, nullptr, true); - PRINT_MODULE_USAGE_PARAM_STRING('I', "BATT_SMBUS_BUS_I2C_INTERNAL", nullptr, nullptr, true); - PRINT_MODULE_USAGE_PARAM_STRING('A', "BATT_SMBUS_BUS_ALL", nullptr, nullptr, true); - PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stops the driver."); - PRINT_MODULE_USAGE_COMMAND_DESCR("suspend", "Suspends the drive but does stop it completely."); - PRINT_MODULE_USAGE_COMMAND_DESCR("resume", "Resumes the driver from the suspended state."); - PRINT_MODULE_USAGE_COMMAND_DESCR("man_nam", "Prints the name of the manufacturer."); - PRINT_MODULE_USAGE_COMMAND_DESCR("man_date", "Prints the date of manufacture."); - PRINT_MODULE_USAGE_COMMAND_DESCR("serial_num", "Prints the serial number."); - PRINT_MODULE_USAGE_COMMAND_DESCR("sbs_info", "Prints the manufacturer name, date, and serial number."); - PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Prints the last report."); - PRINT_MODULE_USAGE_COMMAND_DESCR("unseal", "Unseals the devices flash memory to enable write_flash commands."); - PRINT_MODULE_USAGE_COMMAND_DESCR("read_word", "Uses the SMbus read-word command."); - PRINT_MODULE_USAGE_ARG("command code", "The SMbus command .", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("man_read", "Uses the SMbus block-read with ManufacturerAccess()."); - PRINT_MODULE_USAGE_ARG("command code", "The SMbus command .", true); - PRINT_MODULE_USAGE_ARG("number of bytes", "Number of bytes to read.", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("read_flash", "Reads 32 bytes from flash starting from the address specified."); - PRINT_MODULE_USAGE_ARG("address", "The address to start reading from. .", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("write_flash", "Writes to flash. The device must first be unsealed with the unseal command."); - PRINT_MODULE_USAGE_ARG("address", "The address to start writing.", true); - 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_COMMAND_DESCR("block_read", "Performs a SMBus block read."); - PRINT_MODULE_USAGE_ARG("command code", "The SMbus command .", true); - PRINT_MODULE_USAGE_ARG("number of bytes", "Number of bytes to read.", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("block_write", "Performs a SMBus block write."); - PRINT_MODULE_USAGE_ARG("command code", "The SMbus command code.", true); - 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); -} - -} //namespace - -int -batt_smbus_main(int argc, char *argv[]) -{ - enum BATT_SMBUS_BUS busid = BATT_SMBUS_BUS_I2C_EXTERNAL; - int myoptind = 1; - int ch; - const char* myoptarg = nullptr; - - // Jump over start/off/etc and look at options first. - while ((ch = px4_getopt(argc, argv, "XIA",&myoptind,&myoptarg)) != EOF) { - switch (ch) { - case 'X': //TODO: add option for starting specific external bus - busid = BATT_SMBUS_BUS_I2C_EXTERNAL; - break; - - case 'I': - busid = BATT_SMBUS_BUS_I2C_INTERNAL; - break; - - case 'A': - busid = BATT_SMBUS_BUS_ALL; - break; - - default: - batt_smbus::usage("unrecognized argument"); - return 0; - } - } - - const char *input = argv[myoptind]; - - if(!input) { - batt_smbus::usage("Please enter an appropriate command."); - return 1; - } - - if (!strcmp(input, "start")) { - return batt_smbus::start(busid); - } - - struct batt_smbus::batt_smbus_bus_option &bus = batt_smbus::find_bus(busid); - - if (!strcmp(input, "stop")) { - delete bus.dev; - bus.dev = nullptr; - return 0; - } - - if (!strcmp(input, "suspend")) { - bus.dev->stop(); - return 0; - } - - if (!strcmp(input, "resume")) { - bus.dev->start(); - return 0; - } - - if (!strcmp(input, "man_name")) { - batt_smbus::manufacturer_name(); - return 0; - } - - if (!strcmp(input, "man_date")) { - batt_smbus::manufacture_date(); - return 0; - } - - if (!strcmp(input, "serial_num")) { - batt_smbus::serial_number(); - return 0; - } - - if (!strcmp(input, "sbs_info")) { - batt_smbus::manufacturer_name(); - batt_smbus::manufacture_date(); - batt_smbus::serial_number(); - return 0; - } - - if (!strcmp(input, "info")) { - bus.dev->info(); - return 0; - } - - if (!strcmp(input, "unseal")) { - bus.dev->unseal(); - return 0; - } - - - - if (!strcmp(argv[2], "read_word")) { - if (argv[3]) { - uint8_t data[2]; - - if (PX4_OK != bus.dev->read_word((uint8_t)atoi(argv[3]), data)) { - PX4_INFO("Register read failed"); - return 1; - - } else { - PX4_INFO("Register value: %d %d", data[1], data[0]); - return 0; - } - } - - return 1; - } - - // cmd code (u16), length (32bytes max) - if (!strcmp(argv[2], "man_read")) { - if (argv[3] && argv[4]) { - uint8_t data[32] = {0}; - uint16_t cmd_code = atoi(argv[3]); - unsigned length = atoi(argv[4]); - - if (length > 32) { - PX4_WARN("Data length out of range: Max 32 bytes"); - return 1; - } - - if (PX4_OK != bus.dev->manufacturer_read(cmd_code, data, length)) { - PX4_INFO("Block write failed"); - return 1; - - } else { - return 0; - } - } - - return 1; - } - - // cmd code (u16), length (32bytes max) - if (!strcmp(argv[2], "block_read")) { - if (argv[3] && argv[4]) { - uint8_t data[32] = {0}; - uint8_t cmd_code = atoi(argv[3]); - unsigned length = atoi(argv[4]); - - if (length > 32) { - PX4_WARN("Data length out of range: Max 32 bytes"); - return 1; - } - - if (PX4_OK != bus.dev->block_read(cmd_code, data, length)) { - PX4_INFO("Block read failed"); - return 1; - - } else { - for (unsigned i = 0; i < length; i++) { - PX4_INFO("%d", data[i]); - } - - return 0; - } - } - - return 1; - } - - // address, length, data1, data2, data3, etc - if (!strcmp(argv[2], "block_write")) { - if (argv[3] && argv[4]) { - uint8_t cmd_code = atoi(argv[3]); - unsigned length = atoi(argv[4]); - uint8_t tx_buf[32] = {0}; - - 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++) { - tx_buf[i] = atoi(argv[5 + i]); - PX4_INFO("Read in: %d", tx_buf[i]); - } - - if (PX4_OK != bus.dev->block_write(cmd_code, tx_buf, length)) { - PX4_INFO("Dataflash read failed"); - return 1; - - } else { - return 0; - } - } - - return 1; - } - - if (!strcmp(argv[2], "read_flash")) { - if (argv[3]) { - uint16_t address = atoi(argv[3]); - uint8_t rx_buf[32] = {0}; - - if (PX4_OK != bus.dev->dataflash_read(address, rx_buf)) { - PX4_INFO("Dataflash read failed"); - return 1; - - } else { - return 0; - } - } - - return 1; - } - - if (!strcmp(argv[2], "write_flash")) { - if (argv[3] && argv[4]) { - uint16_t address = atoi(argv[3]); - unsigned length = atoi(argv[4]); - uint8_t tx_buf[32] = {0}; - - 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++) { - tx_buf[i] = atoi(argv[5 + i]); - } - - if (PX4_OK != bus.dev->dataflash_write(address, tx_buf, length)) { - PX4_INFO("Dataflash write failed: %d", address); - usleep(100000); - return 1; - - } else { - usleep(100000); - return 0; - } - } - } - - batt_smbus::usage("unrecognized argument"); - return 1; -}