diff --git a/src/drivers/batt_smbus/CMakeLists.txt b/src/drivers/batt_smbus/CMakeLists.txt index 74ebd8c6ec1..6956880856f 100644 --- a/src/drivers/batt_smbus/CMakeLists.txt +++ b/src/drivers/batt_smbus/CMakeLists.txt @@ -35,6 +35,8 @@ px4_add_module( MAIN batt_smbus COMPILE_FLAGS SRCS + batt_smbus_main.cpp batt_smbus.cpp + batt_smbus_i2c.cpp DEPENDS ) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 40dd0f78f47..e368ae3adbb 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -39,560 +39,258 @@ * * @author Randy Mackay * @author Alex Klimaj + * @author Mark Sauder + * @author Jacob Dahl */ -#include -#include -#include -#include +#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "batt_smbus.h" -#define BATT_SMBUS_ADDR_MIN 0x00 ///< lowest possible address -#define BATT_SMBUS_ADDR_MAX 0xFF ///< highest possible address - -#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION -#define BATT_SMBUS_ADDR 0x0B ///< Default 7 bit address I2C address. 8 bit = 0x16 -#define BATT_SMBUS_TEMP 0x08 ///< temperature register -#define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register -#define BATT_SMBUS_REMAINING_CAPACITY 0x0F ///< predicted remaining battery capacity as a percentage -#define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged -#define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register -#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register -#define BATT_SMBUS_MANUFACTURE_DATE 0x1B ///< manufacture date register -#define BATT_SMBUS_SERIAL_NUMBER 0x1C ///< serial number register -#define BATT_SMBUS_MANUFACTURER_NAME 0x20 ///< manufacturer name -#define BATT_SMBUS_CURRENT 0x0A ///< current register -#define BATT_SMBUS_AVERAGE_CURRENT 0x0B ///< current register -#define BATT_SMBUS_MEASUREMENT_INTERVAL_US (1000000 / 10) ///< time in microseconds, measure at 10Hz -#define BATT_SMBUS_TIMEOUT_US 10000000 ///< timeout looking for battery 10seconds after startup -#define BATT_SMBUS_CYCLE_COUNT 0x17 ///< number of cycles the battery has experienced -#define BATT_SMBUS_RUN_TIME_TO_EMPTY 0x11 ///< predicted remaining battery capacity based on the present rate of discharge in min -#define BATT_SMBUS_AVERAGE_TIME_TO_EMPTY 0x12 ///< predicted remaining battery capacity based on the present rate of discharge in min - -#define BATT_SMBUS_MANUFACTURER_ACCESS 0x00 -#define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS 0x44 - -#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(); - - /** - * Get the SBS manufacturer name of the battery device - * - * @param manufacturer_name pointer a buffer into which the manufacturer name is to be written - * @param max_length the maximum number of bytes to attempt to read from the manufacturer name register, including the null character that is appended to the end - * - * @return the number of bytes read - */ - uint8_t manufacturer_name(uint8_t *man_name, uint8_t max_length); - - /** - * Return the SBS manufacture date of the battery device - * - * @return the date in the following format: - * see Smart Battery Data Specification, Revision 1.1 - * http://sbs-forum.org/specs/sbdat110.pdf for more details - * Date as uint16_t = (year-1980) * 512 + month * 32 + day - * | Field | Bits | Format | Allowable Values | - * | ----- | ---- | ------------------ | ------------------------------------------ | - * | Day 0-4 5-bit binary value 1-31 (corresponds to day) | - * | Month 5-8 4-bit binary value 1-12 (corresponds to month number) | - * | Year 9-15 7-bit binary value 0-127 (corresponds to year biased by 1980) | - * otherwise, return 0 on failure - */ - uint16_t manufacture_date(); - - /** - * Return the SBS serial number of the battery device - */ - uint16_t serial_number(); - -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); - - /** - * Write a word to specified register - */ - int write_reg(uint8_t reg, uint16_t val); - - /** - * Convert from 2's compliment to decimal - * @return the absolute value of the input in decimal - */ - uint16_t convert_twos_comp(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); - - /** - * Write block to the bus - * @return the number of characters sent if successful, zero if unsuccessful - */ - uint8_t write_block(uint8_t reg, uint8_t *data, uint8_t len); - - /** - * 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); - - /** - * Write a word to Manufacturer Access register (0x00) - * @param cmd the word to be written to Manufacturer Access - */ - uint8_t ManufacturerAccess(uint16_t cmd); - - // internal variables - bool _enabled; ///< true if we have successfully connected to battery - work_s _work{}; ///< work queue for scheduling reads - - 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 - - uint64_t _start_time; ///< system time we first attempt to communicate with battery - uint16_t _batt_capacity; ///< battery's design capacity in mAh (0 means unknown) - uint16_t _batt_startup_capacity; ///< battery's remaining capacity on startup - char *_manufacturer_name; ///< The name of the battery manufacturer - uint16_t _cycle_count; ///< number of cycles the battery has experienced - uint16_t _serial_number; ///< serial number register - float _crit_thr; ///< Critical battery threshold param - float _low_thr; ///< Low battery threshold param - float _emergency_thr; ///< Emergency battery threshold param -}; - -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[]); - -int manufacturer_name(); -int manufacture_date(); -int serial_number(); - -BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : - I2C("batt_smbus", "/dev/batt_smbus0", bus, batt_smbus_addr, 100000), - _enabled(false), - _last_report{}, +BATT_SMBUS::BATT_SMBUS(device::Device *interface, const char *path) : + CDev("BATT_SMBUS", path), + _interface(interface), _batt_topic(nullptr), _batt_orb_id(nullptr), - _start_time(0), _batt_capacity(0), _batt_startup_capacity(0), - _manufacturer_name(nullptr), _cycle_count(0), _serial_number(0), _crit_thr(0.0f), + _emergency_thr(0.0f), _low_thr(0.0f), - _emergency_thr(0.0f) + _manufacturer_name(nullptr) { - // capture startup time - _start_time = hrt_absolute_time(); + // Set the device type from the interface. + _device_id.devid_s.bus_type = _interface->get_device_bus_type(); + _device_id.devid_s.bus = _interface->get_device_bus(); + _device_id.devid_s.address = _interface->get_device_address(); } BATT_SMBUS::~BATT_SMBUS() { - // make sure we are truly inactive + // Ensure we are truly inactive. stop(); if (_manufacturer_name != nullptr) { delete[] _manufacturer_name; } + + PX4_WARN("Smart battery driver stopped"); + + int battsource = 0; + param_set(param_find("BAT_SOURCE"), &battsource); } -int -BATT_SMBUS::init() +int BATT_SMBUS::block_read(const uint8_t cmd_code, void *data, const unsigned length) { - int ret = ENOTTY; + unsigned byte_count = 0; + // Length of data (32max). byte_count(1), cmd_code(2), pec(1) + uint8_t rx_data[DATA_BUFFER_SIZE + 4]; - // attempt to initialise I2C bus - ret = I2C::init(); + // If this is a ManufacturerBlockAccess() command then the first 2 data bytes will be the cmd_code. + if (cmd_code == BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS) { + _interface->read(cmd_code, rx_data, length + 4); - if (ret != OK) { - PX4_ERR("failed to init I2C"); - return ret; + byte_count = rx_data[0]; + + // addr1, addr2, byte_count, + memcpy(data, &rx_data[3], byte_count); } else { - //Find the battery on the bus - search(); + // byte_count(1) + data[32max] + pec(1) + _interface->read(cmd_code, rx_data, length + 2); - // start work queue - start(); + byte_count = rx_data[0]; + + memcpy(data, &rx_data[1], byte_count); } - // init orb id - _batt_orb_id = ORB_ID(battery_status); + // addr(wr), cmd_code, addr(r), byte_count, rx_data[] + uint8_t device_address = get_device_address(); + uint8_t full_data_packet[DATA_BUFFER_SIZE + 4] = {0}; - return ret; -} + full_data_packet[0] = (device_address << 1) | 0x00; + full_data_packet[1] = cmd_code; + full_data_packet[2] = (device_address << 1) | 0x01; + full_data_packet[3] = byte_count; -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(); + memcpy(&full_data_packet[4], &rx_data[1], byte_count); - // loop for 3 seconds - while ((hrt_absolute_time() - start_time) < 3000000) { + uint8_t pec = get_pec(full_data_packet, byte_count + 4); - // 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) { - print_message(status); - } - } - - // sleep for 0.2 seconds - usleep(200000); - } - - return OK; -} - -int -BATT_SMBUS::search() -{ - bool found_slave = false; - uint16_t tmp; - int16_t orig_addr = get_device_address(); - - // search through all valid SMBus addresses - for (uint8_t i = BATT_SMBUS_ADDR_MIN; i < BATT_SMBUS_ADDR_MAX; i++) { - set_device_address(i); - - if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { - if (tmp > 0) { - PX4_INFO("battery found at 0x%x", get_device_address()); - found_slave = true; - break; - } - } - - // short sleep - usleep(1); - } - - if (found_slave == false) { - // restore original i2c address - set_device_address(orig_addr); - } - - // display completion message - if (found_slave) { - PX4_INFO("smart battery connected"); + // First byte is byte count, followed by data. + if (pec != ((uint8_t *)rx_data)[byte_count + 1]) { + PX4_INFO("bad PEC from block_read"); + return PX4_ERROR; } else { - PX4_INFO("No smart batteries found."); + return PX4_OK; } - - return OK; } -uint8_t -BATT_SMBUS::manufacturer_name(uint8_t *man_name, uint8_t max_length) +int BATT_SMBUS::block_write(const uint8_t cmd_code, void *data, const unsigned byte_count) { - uint8_t len = read_block(BATT_SMBUS_MANUFACTURER_NAME, man_name, max_length, false); + // cmd code, byte count, data[byte_count], pec + uint8_t buf[byte_count + 2]; - if (len > 0) { - if (len >= max_length - 1) { - man_name[max_length - 1] = 0; + buf[0] = cmd_code; + buf[1] = (uint8_t)byte_count; + memcpy(&buf[2], data, byte_count); + + uint8_t pec = get_pec(buf, sizeof(buf)); + buf[byte_count + 2] = pec; + + unsigned i = 0; + + // If block_write fails, try up to 10 times. + while (i < 10) { + if (PX4_OK != _interface->write(0, buf, sizeof(buf))) { + i++; + PX4_WARN("block_write failed: %d", i); + usleep(100000); } else { - man_name[len] = 0; + return PX4_OK; } } - return len; + return PX4_ERROR; } -uint16_t -BATT_SMBUS::manufacture_date() +void BATT_SMBUS::cycle() { - uint16_t man_date; - - if (read_reg(BATT_SMBUS_MANUFACTURE_DATE, man_date) == OK) { - return man_date; - } - - // Return 0 if could not read the date correctly - return 0; -} - -uint16_t -BATT_SMBUS::serial_number() -{ - uint16_t serial_num; - - if (read_reg(BATT_SMBUS_SERIAL_NUMBER, serial_num) == OK) { - return serial_num; - } - - return -1; -} - -int -BATT_SMBUS::probe() -{ - // always return OK to ensure device starts - return OK; -} - -void -BATT_SMBUS::start() -{ - // 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() -{ - // get current time + // Get the current time. uint64_t now = hrt_absolute_time(); - // exit without rescheduling if we have failed to find a battery after 10 seconds - if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_US)) { - PX4_INFO("did not find smart battery"); - return; - } - - // Try and get battery SBS info - if (_manufacturer_name == nullptr) { - char man_name[21]; - uint8_t len = manufacturer_name((uint8_t *)man_name, sizeof(man_name)); - - if (len > 0) { - _manufacturer_name = new char[len + 1]; - strcpy(_manufacturer_name, man_name); - } - } - - // read battery serial number on startup - if (_serial_number == 0) { - _serial_number = serial_number(); - } - - // temporary variable for storing SMBUS reads - uint16_t tmp; - - // read battery capacity on startup - if (_batt_startup_capacity == 0) { - if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { - _batt_startup_capacity = tmp; - } - } - - // read battery cycle count on startup - if (_cycle_count == 0) { - if (read_reg(BATT_SMBUS_CYCLE_COUNT, tmp) == OK) { - _cycle_count = tmp; - } - } - - // read battery design capacity on startup - if (_batt_capacity == 0) { - if (read_reg(BATT_SMBUS_FULL_CHARGE_CAPACITY, tmp) == OK) { - _batt_capacity = tmp; - } - } - - // read battery threshold params on startup - if (_crit_thr < 0.01f) { - param_get(param_find("BAT_CRIT_THR"), &_crit_thr); - } - - if (_low_thr < 0.01f) { - param_get(param_find("BAT_LOW_THR"), &_low_thr); - } - - if (_emergency_thr < 0.01f) { - param_get(param_find("BAT_EMERGEN_THR"), &_emergency_thr); - } - - // read data from sensor + // Read data from sensor. battery_status_s new_report = {}; - // set time of reading + // Set time of reading. new_report.timestamp = now; - if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { + // Don't publish if any reads fail. + bool success = true; + + // Temporary variable for storing SMBUS reads. + uint16_t tmp; + + if (read_word(BATT_SMBUS_VOLTAGE, &tmp) == PX4_OK) { new_report.connected = true; - // convert millivolts to volts + // Convert millivolts to volts. new_report.voltage_v = ((float)tmp) / 1000.0f; new_report.voltage_filtered_v = new_report.voltage_v; - // read current - if (read_reg(BATT_SMBUS_CURRENT, tmp) == OK) { - new_report.current_a = ((float)convert_twos_comp(tmp)) / 1000.0f; + // Read current. + if (read_word(BATT_SMBUS_CURRENT, &tmp) == PX4_OK) { + new_report.current_a = ((float)(*(int16_t *)&tmp)) / 1000.0f; new_report.current_filtered_a = new_report.current_a; + + } else { + success = false; } - // read average current - if (read_reg(BATT_SMBUS_AVERAGE_CURRENT, tmp) == OK) { - new_report.average_current_a = ((float)convert_twos_comp(tmp)) / 1000.0f; + // Read average current. + if (read_word(BATT_SMBUS_AVERAGE_CURRENT, &tmp) == PX4_OK) { + new_report.average_current_a = ((float)(*(int16_t *)&tmp)) / 1000.0f; + + } else { + success = false; } - // read run time to empty - if (read_reg(BATT_SMBUS_RUN_TIME_TO_EMPTY, tmp) == OK) { + // Read run time to empty. + if (read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, &tmp) == PX4_OK) { new_report.run_time_to_empty = tmp; + + } else { + success = false; } - // read average time to empty - if (read_reg(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, tmp) == OK) { + // Read average time to empty. + if (read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, &tmp) == PX4_OK) { new_report.average_time_to_empty = tmp; + + } else { + success = false; } - // read remaining capacity - if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { + // Read remaining capacity. + if (read_word(BATT_SMBUS_REMAINING_CAPACITY, &tmp) == PX4_OK) { if (tmp > _batt_capacity) { - PX4_WARN("Remaining Cap greater than total: Cap:%hu RemainingCap:%hu", (uint16_t)_batt_capacity, (uint16_t)tmp); + PX4_WARN("Remaining capacity greater than total: Capacity:%hu \tRemaining Capacity:%hu", + (uint16_t)_batt_capacity, (uint16_t)tmp); _batt_capacity = (uint16_t)tmp; } // Calculate remaining capacity percent with complementary filter - new_report.remaining = (float)(_last_report.remaining * 0.8f) + (float)(0.2f * (float)(1.000f - ((( - float)_batt_capacity - (float)tmp) / (float)_batt_capacity))); + new_report.remaining = ((float)_last_report.remaining * 0.8f) + (0.2f * (1.0f - + (((float)_batt_capacity - (float)tmp) / (float)_batt_capacity))); - // calculate total discharged amount - new_report.discharged_mah = (float)((float)_batt_startup_capacity - (float)tmp); - } + // Calculate total discharged amount. + new_report.discharged_mah = (float)_batt_startup_capacity - (float)tmp; - // read battery temperature and covert to Celsius - if (read_reg(BATT_SMBUS_TEMP, tmp) == OK) { - new_report.temperature = (float)(((float)tmp / 10.0f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS); - } - - //Check if remaining % is out of range - if ((new_report.remaining > 1.00f) || (new_report.remaining <= 0.00f)) { - new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; - } - - //Check if discharged amount is greater than the starting capacity - else if (new_report.discharged_mah > (float)_batt_startup_capacity) { - new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; - } - - // propagate warning state - else { - if (new_report.remaining > _low_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_NONE; - - } else if (new_report.remaining > _crit_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_LOW; - - } else if (new_report.remaining > _emergency_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; - - } else { + // Check if remaining % is out of range. + if ((new_report.remaining > 1.00f) || (new_report.remaining <= 0.00f)) { new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + PX4_INFO("Percent out of range: %4.2f", (double)new_report.remaining); } + + // Check if discharged amount is greater than the starting capacity. + else if (new_report.discharged_mah > (float)_batt_startup_capacity) { + new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + PX4_INFO("Discharged greater than startup capacity: %4.2f", (double)new_report.discharged_mah); + } + + // Propagate warning state. + else { + if (new_report.remaining > _low_thr) { + new_report.warning = battery_status_s::BATTERY_WARNING_NONE; + + } else if (new_report.remaining > _crit_thr) { + new_report.warning = battery_status_s::BATTERY_WARNING_LOW; + + } else if (new_report.remaining > _emergency_thr) { + new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + + } else { + uint64_t timer = hrt_absolute_time() - now; + new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + + /* Only warn every 5 seconds */ + if (timer > 5000000) { + PX4_WARN("Battery Warning Emergency: %4.2f", (double)new_report.remaining); + } + } + } + + } else { + success = false; + } + + // Read battery temperature and covert to Celsius. + if (read_word(BATT_SMBUS_TEMP, &tmp) == PX4_OK) { + new_report.temperature = (float)(((float)tmp / 10.0f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS); + + } else { + success = false; } new_report.capacity = _batt_capacity; new_report.cycle_count = _cycle_count; new_report.serial_number = _serial_number; - // publish to orb - if (_batt_topic != nullptr) { + // Publish to orb. + if (_batt_topic != nullptr && success) { + orb_publish(_batt_orb_id, _batt_topic, &new_report); + // Copy report for test(). + _last_report = new_report; + + } else { _batt_topic = orb_advertise(_batt_orb_id, &new_report); @@ -602,375 +300,337 @@ BATT_SMBUS::cycle() } } - // copy report for test() - _last_report = new_report; - - // record we are working - _enabled = true; } - // schedule a fresh cycle call when the measurement is done + // 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)); } -int -BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) +void BATT_SMBUS::cycle_trampoline(void *arg) { - uint8_t buff[3]; // 2 bytes of data - - // 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 { - PX4_ERR("BATT_SMBUS PEC Check Failed"); - ret = ENOTTY; - } - } - - // return success or failure - return ret; + BATT_SMBUS *dev = (BATT_SMBUS *)arg; + dev->cycle(); } -int -BATT_SMBUS::write_reg(uint8_t reg, uint16_t val) +int BATT_SMBUS::dataflash_read(uint16_t &address, void *data) { - uint8_t buff[4]; // reg + 2 bytes of data + PEC + uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS; - buff[0] = reg; - buff[2] = uint8_t(val << 8) & 0xff; - buff[1] = (uint8_t)val; - buff[3] = get_PEC(reg, false, &buff[1], 2); // Append PEC + // address is 2 bytes + block_write(code, &address, 2); + // @NOTE: The data buffer MUST be 32 bytes. + block_read(code, data, DATA_BUFFER_SIZE); - // write bytes to register - int ret = transfer(buff, 3, nullptr, 0); - - if (ret != OK) { - PX4_DEBUG("Register write error"); + // for debug only: print out the receieved buffer + for (unsigned i = 0; i < DATA_BUFFER_SIZE ; i++) { + PX4_INFO("%d", ((uint8_t *)data)[i]); } - // return success or failure - return ret; + return PX4_OK; } -uint16_t -BATT_SMBUS::convert_twos_comp(uint16_t val) -{ - if ((val & 0x8000) == 0x8000) { - uint16_t tmp; - tmp = ~val; - tmp = tmp + 1; - return tmp; - } - return val; +int BATT_SMBUS::dataflash_write(uint16_t &address, void *data, const unsigned length) +{ + uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS; + + // @NOTE: The data buffer can be 1 - 32 bytes. The address field is 2 bytes. + uint8_t tx_buf[DATA_BUFFER_SIZE + 2] = {0}; + + tx_buf[0] = ((uint8_t *)&address)[0]; + tx_buf[1] = ((uint8_t *)&address)[1]; + memcpy(&tx_buf[2], data, length); + + // code (1), byte_count (1), addr(2), data(32) + pec + block_write(code, tx_buf, length + 2); + + return PX4_OK; } -uint8_t -BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero) +uint8_t BATT_SMBUS::get_pec(uint8_t *buff, const uint8_t len) { - uint8_t buff[max_len + 2]; // buffer to hold results - - // 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]) { - 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::write_block(uint8_t reg, uint8_t *data, uint8_t len) -{ - uint8_t buff[len + 3]; // buffer to hold results - - usleep(1); - - buff[0] = reg; - buff[1] = len; - memcpy(&buff[2], data, len); - buff[len + 2] = get_PEC(reg, false, &buff[1], len + 1); // Append PEC - - // send bytes - int ret = transfer(buff, len + 3, nullptr, 0); - - // return zero on failure - if (ret != OK) { - PX4_DEBUG("Block write error"); - return 0; - } - - // return success - return len; -} - -uint8_t -BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) -{ - // exit immediately if no data - if (len <= 0) { - return 0; - } - - /** - * Note: The PEC is calculated on all the message bytes. See http://cache.freescale.com/files/32bit/doc/app_note/AN4471.pdf - * and http://www.ti.com/lit/an/sloa132/sloa132.pdf for more details - */ - - // prepare temp buffer for calculating crc - uint8_t tmp_buff_len; - - if (reading) { - tmp_buff_len = len + 3; - - } else { - tmp_buff_len = len + 2; - } - - uint8_t tmp_buff[tmp_buff_len]; - tmp_buff[0] = (uint8_t)get_device_address() << 1; - tmp_buff[1] = cmd; - - if (reading) { - tmp_buff[2] = tmp_buff[0] | (uint8_t)reading; - memcpy(&tmp_buff[3], buff, len); - - } else { - memcpy(&tmp_buff[2], buff, len); - } - - // initialise crc to zero + // Initialise CRC to zero. uint8_t crc = 0; - uint8_t shift_reg = 0; - bool do_invert; + uint8_t shift_register = 0; + bool invert_crc; - // 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]; + // Calculate crc for each byte in the stream. + for (uint8_t i = 0; i < len; i++) { + // Load next data byte into the shift register + shift_register = buff[i]; - // for each bit in the current byte + // Calculate crc for each bit in the current byte. for (uint8_t j = 0; j < 8; j++) { - do_invert = (crc ^ shift_reg) & 0x80; + invert_crc = (crc ^ shift_register) & 0x80; crc <<= 1; - shift_reg <<= 1; + shift_register <<= 1; - if (do_invert) { + if (invert_crc) { crc ^= BATT_SMBUS_PEC_POLYNOMIAL; } } } - // return result return crc; } -uint8_t -BATT_SMBUS::ManufacturerAccess(uint16_t cmd) +int BATT_SMBUS::get_startup_info() { - // write bytes to Manufacturer Access - int ret = write_reg(BATT_SMBUS_MANUFACTURER_ACCESS, cmd); + int result = PX4_ERROR; + const unsigned name_length = 22; - if (ret != OK) { - PX4_WARN("Manufacturer Access error"); + // Try and get battery SBS info. + if (_manufacturer_name == nullptr) { + char man_name[name_length] = {0}; + result = manufacturer_name((uint8_t *)man_name, sizeof(man_name)); + + if (PX4_OK != result) { + PX4_WARN("Failed to get manufacturer name"); + return PX4_ERROR; + } + + _manufacturer_name = new char[sizeof(man_name)]; + strcpy(_manufacturer_name, man_name); } - return ret; -} + // Temporary variable for storing SMBUS reads. + uint16_t tmp = 0; -///////////////////////// shell functions /////////////////////// - -void -batt_smbus_usage() -{ - PX4_INFO("missing command: try 'start', 'test', 'stop', 'search', 'man_name', 'man_date', 'dev_name', 'serial_num', 'dev_chem', 'sbs_info'"); - PX4_INFO("options:"); - PX4_INFO(" -b i2cbus (%d)", BATT_SMBUS_I2C_BUS); - PX4_INFO(" -a addr (0x%x)", BATT_SMBUS_ADDR); -} - -int -manufacturer_name() -{ - uint8_t man_name[21]; - uint8_t len = g_batt_smbus->manufacturer_name(man_name, sizeof(man_name)); - - if (len > 0) { - PX4_INFO("The manufacturer name: %s", man_name); - return OK; - - } else { - PX4_INFO("Unable to read manufacturer name."); + // Read battery serial number on startup. + if (read_word(BATT_SMBUS_SERIAL_NUMBER, &tmp) == PX4_OK) { + _serial_number = tmp; + result = PX4_OK; } - return -1; -} - -int -manufacture_date() -{ - uint16_t man_date = g_batt_smbus->manufacture_date(); - - if (man_date > 0) { - // 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: %d which is %4d-%02d-%02d", man_date, year, month, day); - return OK; - - } else { - PX4_INFO("Unable to read the manufacturer date."); + // Read battery capacity on startup. + if (read_word(BATT_SMBUS_REMAINING_CAPACITY, &tmp) == PX4_OK) { + _batt_startup_capacity = tmp; + result = PX4_OK; } - return -1; + // Read battery cycle count on startup. + if (read_word(BATT_SMBUS_CYCLE_COUNT, &tmp) == PX4_OK) { + _cycle_count = tmp; + result = PX4_OK; + } + + // Read battery design capacity on startup. + if (read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, &tmp) == PX4_OK) { + _batt_capacity = tmp; + result = PX4_OK; + } + + // Read battery threshold params on startup. + param_get(param_find("BAT_CRIT_THR"), &_crit_thr); + param_get(param_find("BAT_LOW_THR"), &_low_thr); + param_get(param_find("BAT_EMERGEN_THR"), &_emergency_thr); + + return result; } -int -serial_number() +uint16_t BATT_SMBUS::get_serial_number() { - uint16_t serial_num = g_batt_smbus->serial_number(); - PX4_INFO("The serial number: 0x%04x (%d in decimal)", serial_num, serial_num); + uint16_t serial_num = 0; - return OK; + if (read_word(BATT_SMBUS_SERIAL_NUMBER, &serial_num) == PX4_OK) { + return serial_num; + } + + return PX4_ERROR; } -int -batt_smbus_main(int argc, char *argv[]) +int BATT_SMBUS::info() { - int i2cdevice = BATT_SMBUS_I2C_BUS; - int batt_smbusadr = BATT_SMBUS_ADDR; // 7bit address + print_message(_last_report); + return PX4_OK; +} - int ch; +int BATT_SMBUS::init() +{ + if (PX4_OK != CDev::init()) { + PX4_ERR("CDev init failed"); + return PX4_ERROR; + } - // 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, nullptr, 0); + // Find the battery on the bus and read startup info. + if (search_addresses() != PX4_OK) { + PX4_ERR("Failed to init I2C"); + return PX4_ERROR; + } + + // Retry up to 10 times to read startup info. + for (unsigned i = 0; i < 10; i++) { + if (PX4_OK == get_startup_info()) { break; + } - case 'b': - i2cdevice = strtol(optarg, nullptr, 0); - break; - - default: - batt_smbus_usage(); - return 0; + if (i == 9) { + PX4_ERR("Failed to get battery startup info"); + return PX4_ERROR; } } - if (optind >= argc) { - batt_smbus_usage(); - return 1; + // Initialize the orb ID. + _batt_orb_id = ORB_ID(battery_status); + + // Start the work queue. + start(); + + return PX4_OK; +} + +int BATT_SMBUS::manufacture_date(void *man_date) +{ + uint8_t code = BATT_SMBUS_MANUFACTURE_DATE; + + int result = read_word(code, man_date); + + if (PX4_OK != result) { + PX4_WARN("Could not read manufacturer name"); + return PX4_ERROR; } - const char *verb = argv[optind]; + return PX4_OK; +} - if (!strcmp(verb, "start")) { - if (g_batt_smbus != nullptr) { - PX4_ERR("already started"); - return 1; +int BATT_SMBUS::manufacturer_name(uint8_t *man_name, const uint8_t length) +{ + uint8_t code = BATT_SMBUS_MANUFACTURER_NAME; + uint8_t rx_buf[21] = {0}; + + // Returns 21 bytes, add 1 byte for null terminator. + int result = block_read(code, rx_buf, length - 1); + + memcpy(man_name, rx_buf, sizeof(rx_buf)); + + man_name[21] = '\0'; + + if (PX4_OK != result) { + PX4_WARN("Could not read manufacturer name"); + return PX4_ERROR; + } + + return PX4_OK; +} + +int BATT_SMBUS::manufacturer_read(const uint16_t cmd_code, void *data, const unsigned length) +{ + uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS; + + uint8_t address[2] = {0}; + address[0] = ((uint8_t *)&cmd_code)[0]; + address[1] = ((uint8_t *)&cmd_code)[1]; + + block_write(code, address, sizeof(address)); + + // returns the 2 bytes of addr info + data[] + block_read(code, data, length); + + uint8_t rx_buf[DATA_BUFFER_SIZE] = {0}; + memcpy(rx_buf, data, DATA_BUFFER_SIZE); + + PX4_INFO("Received data: %d %d %d %d %d %d %d %d %d %d %d %d", rx_buf[0], rx_buf[1], rx_buf[2], rx_buf[3], rx_buf[4], + rx_buf[5], rx_buf[6], rx_buf[7], rx_buf[8], rx_buf[9], rx_buf[10], rx_buf[11]); + + return PX4_OK; +} + +int BATT_SMBUS::manufacturer_write(const uint16_t cmd_code, void *data, const unsigned length) +{ + uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS; + + uint8_t address[2] = {0}; + address[0] = ((uint8_t *)&cmd_code)[0]; + address[1] = ((uint8_t *)&cmd_code)[1]; + + uint8_t tx_buf[DATA_BUFFER_SIZE + 2] = {0}; + memcpy(tx_buf, address, 2); + memcpy(&tx_buf[2], data, length); + + block_write(code, tx_buf, length + 2); + + return PX4_OK; +} + +int BATT_SMBUS::read_word(const uint8_t cmd_code, void *data) +{ + // 2 data bytes + pec byte + int result = _interface->read(cmd_code, data, 3); + + if (PX4_OK == result) { + // Check PEC. + uint8_t addr = (get_device_address() << 1); + uint8_t full_data_packet[5]; + full_data_packet[0] = addr | 0x00; + full_data_packet[1] = cmd_code; + full_data_packet[2] = addr | 0x01; + memcpy(&full_data_packet[3], data, 2); + + uint8_t pec = get_pec(full_data_packet, sizeof(full_data_packet)); + + if (pec == ((uint8_t *)data)[2]) { + return PX4_OK; } else { - // create new global object - g_batt_smbus = new BATT_SMBUS(i2cdevice, batt_smbusadr); + return PX4_ERROR; + } + } - if (g_batt_smbus == nullptr) { - PX4_ERR("new failed"); - return 1; + return PX4_ERROR; +} + +int BATT_SMBUS::search_addresses() +{ + uint16_t tmp; + + set_device_address(BATT_SMBUS_ADDR); + + if (PX4_OK != read_word(BATT_SMBUS_VOLTAGE, &tmp)) { + // Search through all valid SMBus addresses. + for (uint8_t i = BATT_SMBUS_ADDR_MIN; i < BATT_SMBUS_ADDR_MAX; i++) { + set_device_address(i); + + if (read_word(BATT_SMBUS_VOLTAGE, &tmp) == PX4_OK) { + if (tmp > 0) { + break; + } } - if (OK != g_batt_smbus->init()) { - delete g_batt_smbus; - g_batt_smbus = nullptr; - PX4_ERR("init failed"); - return 1; + if (i == BATT_SMBUS_ADDR_MAX - 1) { + PX4_WARN("No smart batteries found."); + return PX4_ERROR; } } - - return 0; } - // need the driver past this point - if (g_batt_smbus == nullptr) { - PX4_INFO("not started"); - batt_smbus_usage(); - return 1; - } + PX4_INFO("Smart battery found at 0x%x", get_device_address()); + PX4_INFO("Smart battery connected"); - if (!strcmp(verb, "test")) { - g_batt_smbus->test(); - return 0; - } - - if (!strcmp(verb, "stop")) { - delete g_batt_smbus; - g_batt_smbus = nullptr; - return 0; - } - - if (!strcmp(verb, "search")) { - g_batt_smbus->search(); - return 0; - } - - if (!strcmp(verb, "man_name")) { - manufacturer_name(); - return 0; - } - - if (!strcmp(verb, "man_date")) { - manufacture_date(); - return 0; - } - - if (!strcmp(verb, "serial_num")) { - serial_number(); - return 0; - } - - if (!strcmp(verb, "sbs_info")) { - manufacturer_name(); - manufacture_date(); - serial_number(); - return 0; - } - - batt_smbus_usage(); - return 0; + return PX4_OK; } + +void BATT_SMBUS::start() +{ + // Schedule a cycle to start things. + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1); +} + +void BATT_SMBUS::stop() +{ + // Cancel the work queue. + work_cancel(HPWORK, &_work); +} + +int BATT_SMBUS::unseal() +{ + // See pg85 of bq40z50 technical reference. + uint16_t keys[2] = {0x0414, 0x3672}; + + if (PX4_OK != manufacturer_write(keys[0], &keys[1], 2)) { + PX4_INFO("Failed to unseal device."); + return PX4_ERROR; + } + + return PX4_OK; +} \ No newline at end of file diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h new file mode 100644 index 00000000000..a2442fe9c03 --- /dev/null +++ b/src/drivers/batt_smbus/batt_smbus.h @@ -0,0 +1,322 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * @file batt_smbus.h + * + * Header for a battery monitor connected via SMBus (I2C). + * Designed for BQ40Z50-R1/R2 + * + * @author Randy Mackay + * @author Alex Klimaj + * @author Mark Sauder + * @author Jacob Dahl + */ + + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#define DATA_BUFFER_SIZE 32 + +#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION +#define BATT_SMBUS_CURRENT 0x0A ///< current register +#define BATT_SMBUS_AVERAGE_CURRENT 0x0B ///< current register +#define BATT_SMBUS_ADDR 0x0B ///< Default 7 bit address I2C address. 8 bit = 0x16 +#define BATT_SMBUS_ADDR_MIN 0x00 ///< lowest possible address +#define BATT_SMBUS_ADDR_MAX 0xFF ///< highest possible address +#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC +#define BATT_SMBUS_TEMP 0x08 ///< temperature register +#define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register +#define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged +#define BATT_SMBUS_RUN_TIME_TO_EMPTY 0x11 ///< predicted remaining battery capacity based on the present rate of discharge in min +#define BATT_SMBUS_AVERAGE_TIME_TO_EMPTY 0x12 ///< predicted remaining battery capacity based on the present rate of discharge in min +#define BATT_SMBUS_REMAINING_CAPACITY 0x0F ///< predicted remaining battery capacity as a percentage +#define BATT_SMBUS_CYCLE_COUNT 0x17 ///< number of cycles the battery has experienced +#define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register +#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register +#define BATT_SMBUS_MANUFACTURER_NAME 0x20 ///< manufacturer name +#define BATT_SMBUS_MANUFACTURE_DATE 0x1B ///< manufacture date register +#define BATT_SMBUS_SERIAL_NUMBER 0x1C ///< serial number register +#define BATT_SMBUS_MEASUREMENT_INTERVAL_US 100000 ///< time in microseconds, measure at 10Hz +#define BATT_SMBUS_TIMEOUT_US 1000000 ///< timeout looking for battery 10seconds after startup +#define BATT_SMBUS_MANUFACTURER_ACCESS 0x00 +#define BATT_SMBUS_MANUFACTURER_DATA 0x23 +#define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS 0x44 +#define BATT_SMBUS_SECURITY_KEYS 0x0035 + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +/** + * @brief Nuttshell accessible method to return the driver usage arguments. + */ +void batt_smbus_usage(); + +/** + * @brief Nuttshell accessible method to return the battery manufacture date. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ +int manufacture_date(); + +/** + * @brief Nuttshell accessible method to return the battery manufacturer name. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ +int manufacturer_name(); + +/** + * @brief Nuttshell accessible method to return the battery serial number. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ +int serial_number(); + +extern device::Device *BATT_SMBUS_I2C_interface(int bus); +typedef device::Device *(*BATT_SMBUS_constructor)(int); + +class BATT_SMBUS : public device::CDev +{ +public: + + /** + * @brief Default Constructor. + * @param interface The device communication interface (i2c) + * @param path The device i2c address + */ + BATT_SMBUS(device::Device *interface, const char *path); + + /** + * @brief Default Destructor. + */ + virtual ~BATT_SMBUS(); + + /** + * @brief Sends a block read command. + * @param cmd_code The command code. + * @param data The returned data. + * @param length The number of bytes being read + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int block_read(const uint8_t cmd_code, void *data, const unsigned length); + + /** + * @brief Sends a block write command. + * @param cmd_code The command code. + * @param data The data to be written. + * @param length The number of bytes being written. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int block_write(const uint8_t cmd_code, void *data, const unsigned length); + + /** + * @brief Reads data from flash. + * @param address The address to start the read from. + * @param data The returned data. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int dataflash_read(uint16_t &address, void *data); + + /** + * @brief Writes data to flash. + * @param address The start address of the write. + * @param data The data to be written. + * @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); + + /** + * @brief Calculates the PEC from the data. + * @param buff The buffer that stores the data. + * @param length The number of bytes being written. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + uint8_t get_pec(uint8_t *buffer, const uint8_t length); + + /** + * @brief Returns the SBS serial number of the battery device. + * @return Returns the SBS serial number of the battery device. + */ + uint16_t get_serial_number(); + + /** + * @brief Read info from battery on startup. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int get_startup_info(); + + /** + * @brief Prints the latest report. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int info(); + + /** + * @brief Initializes the smart battery device. Calls probe() to check for device on bus. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + virtual int init(); + + /** + * @brief Gets the SBS manufacture date of the battery. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int manufacture_date(void *man_date); + + /** + * @brief Gets the SBS manufacturer name of the battery device. + * @param manufacturer_name Pointer to a buffer into which the manufacturer name is to be written. + * @param max_length The maximum number of bytes to attempt to read from the manufacturer name register, + * including the null character that is appended to the end. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int manufacturer_name(uint8_t *manufacturer_name, const uint8_t length); + + /** + * @brief Performs a ManufacturerBlockAccess() read command. + * @param cmd_code The command code. + * @param data The returned data. + * @param length The number of bytes being written. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int manufacturer_read(const uint16_t cmd_code, void *data, const unsigned length); + + /** + * @brief Performs a ManufacturerBlockAccess() write command. + * @param cmd_code The command code. + * @param data The sent data. + * @param length The number of bytes being written. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int manufacturer_write(const uint16_t cmd_code, void *data, const unsigned length); + /** + * @brief Sends a read-word command with cmd_code as the command. + * @param cmd_code The command code. + * @param data The returned data. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int read_word(const uint8_t cmd_code, void *data); + + /** + * @brief Search all possible slave addresses for a smart battery. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int search_addresses(); + + /** + * @brief Starts periodic reads from the battery. + */ + void start(); + + /** + * @brief Stops periodic reads from the battery. + */ + void stop(); + + /** + * @brief Unseals the battery to allow writing to restricted flash. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int unseal(); + + +protected: + Device *_interface; + +private: + + /** + * @brief Static function that is called by worker queue. + */ + static void cycle_trampoline(void *arg); + + /** + * @brief The loop that continually generates new reports. + */ + void cycle(); + + /** @struct _work Work queue for scheduling reads. */ + work_s _work = work_s{}; + + /** @param _last_report Last published report, used for test(). */ + battery_status_s _last_report = battery_status_s{}; + + /** @param _batt_topic uORB battery topic. */ + orb_advert_t _batt_topic; + + /** @param _batt_orb_id uORB battery topic ID. */ + orb_id_t _batt_orb_id; + + /** @param _batt_capacity Battery design capacity in mAh (0 means unknown). */ + uint16_t _batt_capacity; + + /** @param _batt_startup_capacity Battery remaining capacity in mAh on startup. */ + uint16_t _batt_startup_capacity; + + /** @param _cycle_count The number of cycles the battery has experienced. */ + uint16_t _cycle_count; + + /** @param _serial_number Serial number register. */ + uint16_t _serial_number; + + /** @param _crit_thr Critical battery threshold param. */ + float _crit_thr; + + /** @param _emergency_thr Emergency battery threshold param. */ + float _emergency_thr; + + /** @param _low_thr Low battery threshold param. */ + float _low_thr; + + /** @param _manufacturer_name Name of the battery manufacturer. */ + char *_manufacturer_name; + + /* Do not allow copy construction or move assignment of this class. */ + BATT_SMBUS(const BATT_SMBUS &); + BATT_SMBUS operator=(const BATT_SMBUS &); +}; \ No newline at end of file diff --git a/src/drivers/batt_smbus/batt_smbus_i2c.cpp b/src/drivers/batt_smbus/batt_smbus_i2c.cpp new file mode 100644 index 00000000000..81eda3e80d1 --- /dev/null +++ b/src/drivers/batt_smbus/batt_smbus_i2c.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file batt_smbus_i2c.cpp + * + * I2C interface for batt_smbus + * + * @author Jacob Dahl + */ + +#include "batt_smbus.h" + +#include + +#include + +#include +#include + +#include "board_config.h" + +device::Device *BATT_SMBUS_I2C_interface(int bus); + +class BATT_SMBUS_I2C : public device::I2C +{ +public: + BATT_SMBUS_I2C(int bus); + virtual ~BATT_SMBUS_I2C() = default; + + /** + * @brief Sends a block read command. + * @param cmd_code The command code. + * @param data Pointer to the data being returned. + * @param count The number of bytes being read + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + virtual int read(unsigned cmd_code, void *data, unsigned count); + + /** + * @brief Sends a block write command. + * @param cmd_code The command code. + * @param data Pointer to the data to be written. + * @param count The number of bytes being written. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + virtual int write(unsigned cmd_code, void *data, unsigned count); + + /** + * @brief Sends a block read command. + * @param cmd_code The command code. + * @param data Pointer to the data being returned. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int read(unsigned cmd_code, void *data); + + + /** + * @brief Sends a block write command. + * @param cmd_code The command code. + * @param data Pointer to the data to be written. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + int write(unsigned cmd_code, void *data); + +protected: + + /** + * @brief Calculates the PEC from the data. + * @param buffer The buffer that stores the data. + * @param length The number of bytes being written. + * @return Returns PX4_OK on success, PX4_ERROR on failure. + */ + uint8_t get_pec(uint8_t *buffer, uint8_t length); + +}; + +device::Device * +BATT_SMBUS_I2C_interface(int bus) +{ + return new BATT_SMBUS_I2C(bus); +} + +BATT_SMBUS_I2C::BATT_SMBUS_I2C(int bus) : + I2C("BATT_SMBUS_I2C", nullptr, bus, BATT_SMBUS_ADDR, 100000) +{ +} + + +int +BATT_SMBUS_I2C::read(unsigned cmd_code, void *data, unsigned length) +{ + uint8_t buf = (uint8_t) cmd_code; + return transfer(&buf, 1, (uint8_t *)data, length); +} + +int +BATT_SMBUS_I2C::write(unsigned cmd_code, void *data, unsigned length) +{ + return transfer((uint8_t *)data, length, nullptr, 0); +} \ No newline at end of file diff --git a/src/drivers/batt_smbus/batt_smbus_main.cpp b/src/drivers/batt_smbus/batt_smbus_main.cpp new file mode 100644 index 00000000000..f930c9e0abe --- /dev/null +++ b/src/drivers/batt_smbus/batt_smbus_main.cpp @@ -0,0 +1,530 @@ +/**************************************************************************** + * + * 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 + +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[] = { + { BATT_SMBUS_BUS_I2C_EXTERNAL, "/dev/batt_smbus_ext", &BATT_SMBUS_I2C_interface, PX4_I2C_BUS_EXPANSION, nullptr }, +#ifdef PX4_I2C_BUS_ONBOARD + { BATT_SMBUS_BUS_I2C_INTERNAL, "/dev/batt_smbus_int", &BATT_SMBUS_I2C_interface, PX4_I2C_BUS_ONBOARD, 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 %d 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"); + 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 +GPS driver module that handles the communication with the device and publishes the position via uORB. +It supports multiple protocols (device vendors) and by default automatically selects the correct one. + +The module supports a secondary GPS device, specified via `-e` parameter. The position will be published +on the second uORB topic instance, but it's currently not used by the rest of the system (however the +data will be logged, so that it can be used for comparisons). + +### Implementation +There is a thread for each device polling for data. The GPS protocol classes are implemented with callbacks +so that they can be used in other projects as well (eg. QGroundControl uses them too). + +### Examples +For testing it can be useful to fake a GPS signal (it will signal the system that it has a valid position): +$ gps stop +$ gps start -f +)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 ch; + + + // Jump over start/off/etc and look at options first. + while ((ch = getopt(argc, argv, "XIA:")) != EOF) { + switch (ch) { + case 'X': + 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[optind]; + + 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; +} \ No newline at end of file