From d3b54c359949e05af4384fd5ebfa9b70527a0d51 Mon Sep 17 00:00:00 2001
From: Jake Dahl <jacob.dahl@tealdrones.com>
Date: Thu, 11 Oct 2018 09:35:21 -0600
Subject: [PATCH] more changes based on suggestions
---
src/lib/drivers/smbus/SMBus.cpp | 51 +++++++++++----------------------
1 file changed, 17 insertions(+), 34 deletions(-)
diff --git a/src/lib/drivers/smbus/SMBus.cpp b/src/lib/drivers/smbus/SMBus.cpp
index c04fedcacf..2b563aca9d 100644
--- a/src/lib/drivers/smbus/SMBus.cpp
+++ b/src/lib/drivers/smbus/SMBus.cpp
@@ -69,33 +69,23 @@ int SMBus::read_word(const uint8_t cmd_code, void *data)
int SMBus::block_read(const uint8_t cmd_code, void *data, const uint8_t length, bool use_pec)
{
unsigned byte_count = 0;
- // Length of data (32max). byte_count(1), cmd_code(2) (sometimes), pec(1) (optional)
- uint8_t rx_data[32 + 4];
+ // addr(wr), cmd_code, addr(r), byte_count, data (32 bytes max), pec
+ uint8_t rx_data[32 + 5];
- int result = transfer(&cmd_code, 1, (uint8_t *)rx_data, length + 2);
+ int result = transfer(&cmd_code, 1, (uint8_t *)&rx_data[3], length + 2);
- byte_count = rx_data[0];
+ uint8_t device_address = get_device_address();
+ rx_data[0] = (device_address << 1) | 0x00;
+ rx_data[1] = cmd_code;
+ rx_data[2] = (device_address << 1) | 0x01;
+ byte_count = rx_data[3];
- // byte_count, data[length], PEC
- memcpy(data, &rx_data[1], byte_count);
+ memcpy(data, &rx_data[4], byte_count);
if (use_pec) {
+ uint8_t pec = get_pec(rx_data, byte_count + 4);
- // addr(wr), cmd_code, addr(r), byte_count, rx_data[]
- uint8_t device_address = get_device_address();
- uint8_t full_data_packet[32 + 4] = {};
-
- 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;
-
- memcpy(&full_data_packet[4], &rx_data[1], byte_count);
-
- uint8_t pec = get_pec(full_data_packet, byte_count + 4);
-
- // First byte is byte count, followed by data.
- if (pec != ((uint8_t *)rx_data)[byte_count + 1]) {
+ if (pec != rx_data[byte_count + 4]) {
result = -EINVAL;
}
}
@@ -122,20 +112,13 @@ int SMBus::block_write(const uint8_t cmd_code, void *data, uint8_t byte_count, b
int result = 0;
// If block_write fails, try up to 10 times.
- while (i < 10) {
- result = transfer((uint8_t *)buf, byte_count + 2, nullptr, 0);
-
- if (result != PX4_OK) {
- i++;
-
- if (i == 10) {
- PX4_WARN("Block_write failed 10 times");
- result = -EINVAL;
- }
+ while (i < 10 && (result = transfer((uint8_t *)buf, byte_count + 2, nullptr, 0)) != PX4_OK) {
+ i++;
+ }
- } else {
- break;
- }
+ if (i == 10) {
+ PX4_WARN("Block_write failed 10 times");
+ result = -EINVAL;
}
return result;
--
GitLab