From 8f7ff29b1f70538d0aac71df8b0a10f22adcb44c Mon Sep 17 00:00:00 2001
From: Mihai Tudor Panu <mihai.tudor.panu@intel.com>
Date: Fri, 28 Oct 2016 16:00:24 -0700
Subject: [PATCH] smartdrive: extended writeArray() function to use proper
 array length

Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>
---
 src/smartdrive/smartdrive.cxx | 26 +++++++++++++-------------
 src/smartdrive/smartdrive.hpp |  2 +-
 2 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/src/smartdrive/smartdrive.cxx b/src/smartdrive/smartdrive.cxx
index 7c945912..12e02b1c 100644
--- a/src/smartdrive/smartdrive.cxx
+++ b/src/smartdrive/smartdrive.cxx
@@ -65,9 +65,9 @@ SmartDrive::readByte(uint8_t addr) {
 }
 
 void
-SmartDrive::writeArray(uint8_t* array) {
+SmartDrive::writeArray(uint8_t* array, uint8_t len) {
     try {
-        m_i2c_smartdrive_control.write(array, sizeof(array)/sizeof(uint8_t));
+        m_i2c_smartdrive_control.write(array, len);
     } catch (int e) {
         std::cout << "Failed to write array values to address " << array[0] << " --> " << e << std::endl;
     }
@@ -144,11 +144,11 @@ SmartDrive::Run_Unlimited(int motor_id, int direction, uint8_t speed) {
             speed = speed * -1;
         if ( motor_id != SmartDrive_Motor_ID_2) {
             uint8_t array [5] = {SmartDrive_SPEED_M1, speed, 0, 0, ctrl};
-            writeArray(array);
+            writeArray(array, sizeof(array));
         }
         if ( motor_id != SmartDrive_Motor_ID_1) {
             uint8_t array [5] = {SmartDrive_SPEED_M2, speed, 0, 0, ctrl};
-            writeArray(array);
+            writeArray(array, sizeof(array));
         }
         if ( motor_id == SmartDrive_Motor_ID_BOTH )
             writeByte(SmartDrive_COMMAND, CMD_S);
@@ -181,11 +181,11 @@ SmartDrive::Run_Seconds(int motor_id, int direction, uint8_t speed, uint8_t dura
             speed = speed * -1;
         if ( motor_id != SmartDrive_Motor_ID_2) {
             uint8_t array[5] = {SmartDrive_SPEED_M1, speed, duration, 0, ctrl};
-            writeArray(array);
+            writeArray(array, sizeof(array));
         }
         if ( motor_id != SmartDrive_Motor_ID_1) {
             uint8_t array[5] = {SmartDrive_SPEED_M2, speed, duration, 0, ctrl};
-            writeArray(array);
+            writeArray(array, sizeof(array));
         }
         if ( motor_id == SmartDrive_Motor_ID_BOTH )
             writeByte(SmartDrive_COMMAND, CMD_S);
@@ -240,11 +240,11 @@ SmartDrive::Run_Degrees(int motor_id, int direction, uint8_t speed, uint32_t deg
             ctrl |= SmartDrive_CONTROL_GO;
         if ( motor_id != SmartDrive_Motor_ID_2) {
             uint8_t array[9] = {SmartDrive_SETPT_M1, t1, t2, t3, t4, speed, 0, 0, ctrl};
-            writeArray(array);
+            writeArray(array, sizeof(array));
         }
         if ( motor_id != SmartDrive_Motor_ID_1){
             uint8_t array[9] = {SmartDrive_SETPT_M2, t1, t2, t3, t4, speed, 0, 0, ctrl};
-            writeArray(array);
+            writeArray(array, sizeof(array));
         }
         if ( motor_id == SmartDrive_Motor_ID_BOTH )
             writeByte(SmartDrive_COMMAND, CMD_S);
@@ -281,11 +281,11 @@ SmartDrive::Run_Rotations(int motor_id, int direction, uint8_t speed, uint32_t r
             ctrl |= SmartDrive_CONTROL_GO;
         if ( motor_id != SmartDrive_Motor_ID_2) {
             uint8_t array[9] = {SmartDrive_SETPT_M1, t1, t2, t3, t4, speed, 0, 0, ctrl};
-            writeArray(array);
+            writeArray(array, sizeof(array));
         }
         if ( motor_id != SmartDrive_Motor_ID_1) {
             uint8_t array[9] = {SmartDrive_SETPT_M2, t1, t2, t3, t4, speed, 0, 0, ctrl};
-            writeArray(array);
+            writeArray(array, sizeof(array));
         }
         if ( motor_id == SmartDrive_Motor_ID_BOTH )
             writeByte(SmartDrive_COMMAND, CMD_S);
@@ -319,11 +319,11 @@ SmartDrive::Run_Tacho(int motor_id, uint8_t speed, uint32_t tacho_count, bool wa
             ctrl |= SmartDrive_CONTROL_GO;
         if ( motor_id != SmartDrive_Motor_ID_2){
             uint8_t array[9]= {SmartDrive_SETPT_M1, t1, t2, t3, t4, speed, 0, 0, ctrl};
-            writeArray(array);
+            writeArray(array, sizeof(array));
         }
         if ( motor_id != SmartDrive_Motor_ID_1){
             uint8_t array[9]= {SmartDrive_SETPT_M2, t1, t2, t3, t4, speed, 0, 0, ctrl};
-            writeArray(array);
+            writeArray(array, sizeof(array));
         }
         if ( motor_id == SmartDrive_Motor_ID_BOTH )
             writeByte(SmartDrive_COMMAND, CMD_S);
@@ -369,7 +369,7 @@ SmartDrive::SetPerformanceParameters( uint16_t Kp_tacho, uint16_t Ki_tacho, uint
     uint8_t Kd_s2 = Kd_speed/0x100;
 
     uint8_t array[15] = {SmartDrive_P_Kp, Kp_t1 , Kp_t2 , Ki_t1, Ki_t2, Kd_t1, Kd_t2, Kp_s1, Kp_s2, Ki_s1, Ki_s2, Kd_s1, Kd_s2, passcount, tolerance};
-    writeArray(array);
+    writeArray(array, sizeof(array));
 }
 
 
diff --git a/src/smartdrive/smartdrive.hpp b/src/smartdrive/smartdrive.hpp
index 72bcb921..0560a469 100644
--- a/src/smartdrive/smartdrive.hpp
+++ b/src/smartdrive/smartdrive.hpp
@@ -288,7 +288,7 @@ public:
 
 private:
     void writeByte(uint8_t addr, uint8_t value);
-    void writeArray(uint8_t* array);
+    void writeArray(uint8_t* array, uint8_t len);
     uint8_t readByte(uint8_t addr);
     uint16_t readInteger(uint8_t addr);
     uint32_t readLongSigned(uint8_t addr);