From 871e9101ff01f26602da826793d24aee8451f4e6 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Thu, 4 Apr 2024 14:05:19 +0100
Subject: [PATCH] AP_Volz_Protocol: add CMD union helper

---
 .../AP_Volz_Protocol/AP_Volz_Protocol.cpp     | 33 +++++++++----------
 libraries/AP_Volz_Protocol/AP_Volz_Protocol.h | 20 +++++++++--
 2 files changed, 32 insertions(+), 21 deletions(-)

diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp
index fdd788b429..211128723e 100644
--- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp
+++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp
@@ -56,8 +56,8 @@ void AP_Volz_Protocol::update()
     }
 
     uint32_t now = AP_HAL::micros();
-    if (now - last_volz_update_time < volz_time_frame_micros ||
-        port->txspace() < VOLZ_DATA_FRAME_SIZE) {
+    if (((now - last_volz_update_time) < volz_time_frame_micros) ||
+        (port->txspace() < sizeof(CMD))) {
         return;
     }
 
@@ -93,31 +93,28 @@ void AP_Volz_Protocol::update()
             }
 
             // prepare Volz protocol data.
-            uint8_t data[VOLZ_DATA_FRAME_SIZE];
+            CMD cmd;
+            cmd.ID = SET_EXTENDED_POSITION_CMD;
+            cmd.actuator_id = i + 1; // send actuator id as 1 based index so ch1 will have id 1, ch2 will have id 2 ....
+            cmd.arg1 = HIGHBYTE(value);
+            cmd.arg2 = LOWBYTE(value);
 
-            data[0] = VOLZ_SET_EXTENDED_POSITION_CMD;
-            data[1] = i + 1;		// send actuator id as 1 based index so ch1 will have id 1, ch2 will have id 2 ....
-            data[2] = HIGHBYTE(value);
-            data[3] = LOWBYTE(value);
-
-            send_command(data);
+            send_command(cmd);
         }
     }
 }
 
 // calculate CRC for volz serial protocol and send the data.
-void AP_Volz_Protocol::send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE])
+void AP_Volz_Protocol::send_command(CMD &cmd)
 {
-    uint8_t i,j;
     uint16_t crc = 0xFFFF;
 
     // calculate Volz CRC value according to protocol definition
-    for(i=0; i<4; i++) {
+    for(uint8_t i=0; i<4; i++) {
         // take input data into message that will be transmitted.
-        crc = ((data[i] << 8) ^ crc);
-
-        for(j=0; j<8; j++) {
+        crc = (cmd.data[i] << 8) ^ crc;
 
+        for(uint8_t j=0; j<8; j++) {
             if (crc & 0x8000) {
                 crc = (crc << 1) ^ 0x8005;
             } else {
@@ -127,9 +124,9 @@ void AP_Volz_Protocol::send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE])
     }
 
     // add CRC result to the message
-    data[4] = HIGHBYTE(crc);
-    data[5] = LOWBYTE(crc);
-    port->write(data, VOLZ_DATA_FRAME_SIZE);
+    cmd.crc1 = HIGHBYTE(crc);
+    cmd.crc2 = LOWBYTE(crc);
+    port->write(cmd.data, sizeof(cmd));
 }
 
 void AP_Volz_Protocol::update_volz_bitmask(uint32_t new_bitmask)
diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h
index c4f5705bd0..5497357342 100644
--- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h
+++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h
@@ -64,14 +64,28 @@ public:
     CLASS_NO_COPY(AP_Volz_Protocol);
 
     static const struct AP_Param::GroupInfo var_info[];
-    
+
     void update();
 
 private:
+
+    // Command frame
+    union CMD {
+        struct PACKED {
+            uint8_t ID; // CMD ID
+            uint8_t actuator_id; // actuator send to or receiving from
+            uint8_t arg1; // CMD dependant argument 1
+            uint8_t arg2; // CMD dependant argument 2
+            uint8_t crc1;
+            uint8_t crc2;
+        };
+        uint8_t data[6];
+    };
+
     AP_HAL::UARTDriver *port;
-    
+
     void init(void);
-    void send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE]);
+    void send_command(CMD &cmd);
     void update_volz_bitmask(uint32_t new_bitmask);
 
     uint32_t last_volz_update_time;