diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index f1edd0c14d..09125206d8 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -174,7 +174,7 @@ void BATT_SMBUS::Run() // Temporary variable for storing SMBUS reads. uint16_t result; - int ret = _interface->read_word(BATT_SMBUS_VOLTAGE, &result); + int ret = _interface->read_word(BATT_SMBUS_VOLTAGE, result); ret |= get_cell_voltages(); @@ -183,13 +183,13 @@ void BATT_SMBUS::Run() new_report.voltage_filtered_v = new_report.voltage_v; // Read current. - ret |= _interface->read_word(BATT_SMBUS_CURRENT, &result); + ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f); new_report.current_filtered_a = new_report.current_a; // Read average current. - ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, &result); + ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result); float average_current = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f); @@ -200,15 +200,15 @@ void BATT_SMBUS::Run() set_undervoltage_protection(average_current); // Read run time to empty. - ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, &result); + ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result); new_report.run_time_to_empty = result; // Read average time to empty. - ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, &result); + ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result); new_report.average_time_to_empty = result; // Read remaining capacity. - ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, &result); + ret |= _interface->read_word(BATT_SMBUS_REMAINING_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) / @@ -239,7 +239,7 @@ void BATT_SMBUS::Run() } // Read battery temperature and covert to Celsius. - ret |= _interface->read_word(BATT_SMBUS_TEMP, &result); + ret |= _interface->read_word(BATT_SMBUS_TEMP, result); new_report.temperature = ((float)result / 10.0f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; new_report.capacity = _batt_capacity; @@ -287,19 +287,19 @@ int BATT_SMBUS::get_cell_voltages() // Temporary variable for storing SMBUS reads. uint16_t result = 0; - int ret = _interface->read_word(BATT_SMBUS_CELL_1_VOLTAGE, &result); + int ret = _interface->read_word(BATT_SMBUS_CELL_1_VOLTAGE, result); // Convert millivolts to volts. _cell_voltages[0] = ((float)result) / 1000.0f; - ret = _interface->read_word(BATT_SMBUS_CELL_2_VOLTAGE, &result); + ret = _interface->read_word(BATT_SMBUS_CELL_2_VOLTAGE, result); // Convert millivolts to volts. _cell_voltages[1] = ((float)result) / 1000.0f; - ret = _interface->read_word(BATT_SMBUS_CELL_3_VOLTAGE, &result); + ret = _interface->read_word(BATT_SMBUS_CELL_3_VOLTAGE, result); // Convert millivolts to volts. _cell_voltages[2] = ((float)result) / 1000.0f; - ret = _interface->read_word(BATT_SMBUS_CELL_4_VOLTAGE, &result); + ret = _interface->read_word(BATT_SMBUS_CELL_4_VOLTAGE, result); // Convert millivolts to volts. _cell_voltages[3] = ((float)result) / 1000.0f; @@ -414,20 +414,17 @@ int BATT_SMBUS::get_startup_info() _manufacturer_name = new char[sizeof(man_name)]; } - // Temporary variable for storing SMBUS reads. - uint16_t tmp = 0; + uint16_t serial_num; + result = _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num); - result = _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, &tmp); - uint16_t serial_num = tmp; + uint16_t remaining_cap; + result |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, remaining_cap); - result |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, &tmp); - uint16_t remaining_cap = tmp; + uint16_t cycle_count; + result |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, cycle_count); - result |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, &tmp); - uint16_t cycle_count = tmp; - - result |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, &tmp); - uint16_t full_cap = tmp; + uint16_t full_cap; + result |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, full_cap); if (!result) { _serial_number = serial_num; @@ -463,7 +460,7 @@ uint16_t BATT_SMBUS::get_serial_number() { uint16_t serial_num = 0; - if (_interface->read_word(BATT_SMBUS_SERIAL_NUMBER, &serial_num) == PX4_OK) { + if (_interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num) == PX4_OK) { return serial_num; } @@ -472,10 +469,8 @@ uint16_t BATT_SMBUS::get_serial_number() int BATT_SMBUS::manufacture_date() { - uint16_t date = PX4_ERROR; - uint8_t code = BATT_SMBUS_MANUFACTURE_DATE; - - int result = _interface->read_word(code, &date); + uint16_t date; + int result = _interface->read_word(BATT_SMBUS_MANUFACTURE_DATE, date); if (result != PX4_OK) { return result; @@ -550,9 +545,9 @@ int BATT_SMBUS::unseal() // See bq40z50 technical reference. uint16_t keys[2] = {0x0414, 0x3672}; - int ret = _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, &keys[0]); + int ret = _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, keys[0]); - ret |= _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, &keys[1]); + ret |= _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, keys[1]); return ret; } diff --git a/src/lib/drivers/smbus/SMBus.cpp b/src/lib/drivers/smbus/SMBus.cpp index c175aefd1c..e10f1633a5 100644 --- a/src/lib/drivers/smbus/SMBus.cpp +++ b/src/lib/drivers/smbus/SMBus.cpp @@ -54,23 +54,23 @@ SMBus::~SMBus() { } -int SMBus::read_word(const uint8_t cmd_code, void *data) +int SMBus::read_word(const uint8_t cmd_code, uint16_t &data) { + uint8_t buf[6]; // 2 data bytes + pec byte - int result = transfer(&cmd_code, 1, (uint8_t *)data, 3); + int result = transfer(&cmd_code, 1, buf + 3, 3); if (result == PX4_OK) { + data = buf[3] | ((uint16_t)buf[4] << 8); // 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); + buf[0] = addr | 0x00; + buf[1] = cmd_code; + buf[2] = addr | 0x01; - uint8_t pec = get_pec(full_data_packet, sizeof(full_data_packet) / sizeof(full_data_packet[0])); + uint8_t pec = get_pec(buf, sizeof(buf) - 1); - if (pec != ((uint8_t *)data)[2]) { + if (pec != buf[sizeof(buf) - 1]) { result = -EINVAL; } } @@ -78,14 +78,14 @@ int SMBus::read_word(const uint8_t cmd_code, void *data) return result; } -int SMBus::write_word(const uint8_t cmd_code, void *data) +int SMBus::write_word(const uint8_t cmd_code, uint16_t data) { // 2 data bytes + pec byte - uint8_t buf[5] = {}; + uint8_t buf[5]; buf[0] = (get_device_address() << 1) | 0x10; buf[1] = cmd_code; - buf[2] = ((uint8_t *)data)[0]; - buf[3] = ((uint8_t *)data)[1]; + buf[2] = data & 0xff; + buf[3] = (data >> 8) & 0xff; buf[4] = get_pec(buf, 4); @@ -177,4 +177,4 @@ uint8_t SMBus::get_pec(uint8_t *buff, const uint8_t len) } return crc; -} \ No newline at end of file +} diff --git a/src/lib/drivers/smbus/SMBus.hpp b/src/lib/drivers/smbus/SMBus.hpp index e6518fac48..9cae85d894 100644 --- a/src/lib/drivers/smbus/SMBus.hpp +++ b/src/lib/drivers/smbus/SMBus.hpp @@ -55,7 +55,7 @@ public: * @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. + * @return Returns PX4_OK on success, -errno on failure. */ int block_write(const uint8_t cmd_code, void *data, uint8_t byte_count, bool use_pec); @@ -64,7 +64,7 @@ public: * @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. + * @return Returns PX4_OK on success, -errno on failure. */ int block_read(const uint8_t cmd_code, void *data, const uint8_t length, bool use_pec); @@ -72,23 +72,23 @@ public: * @brief Sends a read word command. * @param cmd_code The command code. * @param data The 2 bytes of returned data plus a 1 byte CRC if used. - * @return Returns PX4_OK on success, PX4_ERROR on failure. + * @return Returns PX4_OK on success, -errno on failure. */ - int read_word(const uint8_t cmd_code, void *data); + int read_word(const uint8_t cmd_code, uint16_t &data); /** * @brief Sends a write word command. * @param cmd_code The command code. * @param data The 2 bytes of data to be transfered. - * @return Returns PX4_OK on success, PX4_ERROR on failure. + * @return Returns PX4_OK on success, -errno on failure. */ - int write_word(const uint8_t cmd_code, void *data); + int write_word(const uint8_t cmd_code, uint16_t data); /** * @brief Calculates the PEC from the data. * @param buffer The buffer that stores the data to perform the CRC over. * @param length The number of bytes being written. - * @return Returns PX4_OK on success, PX4_ERROR on failure. + * @return Returns PX4_OK on success, -errno on failure. */ uint8_t get_pec(uint8_t *buffer, uint8_t length);