Working new commands

Modified test script and fixed setParam function

quad/scripts/stress_tests.py

@@ -3,6 +3,7 @@
 import sys
 import random
 from time import sleep
+import struct
 
 import serial
 
@@ -29,7 +30,6 @@ def read_packet(ser, raw=False):
     length = int.from_bytes(header[5:7], byteorder='little')
     data = ser.read(length)
     checksum = ser.read()
-    print(checksum)
     if raw:
         return header + data + checksum
     else:
@@ -62,10 +62,8 @@ def check_test(ser, n_sent, size_sent, old_status):
 def test_checksum(ser):
     print("Checking if received checksum is valid")
     # Send query packet
-    ser.write(create_msg(0, 2, 0, b''))
+    ser.write(create_msg(2, 0, b''))
     raw_data = read_packet(ser, True)
-    print("Raw:")
-    print(raw_data.hex())
     given_checksum = raw_data[-1]
     computed_checksum = 0
     for i in range(len(raw_data) - 1):
@@ -108,11 +106,33 @@ def test_bad_checksum(ser, cur_status, size=30):
     return check_test(ser, 0, 0, cur_status)
 
 def test_get_set(ser):
-	print("Checking Get/Set Commands")
-	for cntl_id in range(9):
-		for const_id in range(4):
-			to_set = random.random()
-			#set_packet = create_msg()
+    print("Checking Get/Set Commands...")
+    passed = True
+    for cntl_id in range(9):
+        for const_id in range(4):
+            to_set = random.random()
+            # Construct the set command packet
+            set_data = bytes([cntl_id, const_id])
+            set_data += struct.pack("<f", to_set)
+            set_packet = create_msg(7, 0, set_data)
+            ser.write(set_packet)
+            # Construct the get command packet
+            get_packet = create_msg(8, 0, bytes([cntl_id, const_id]))
+            ser.write(get_packet)
+            # Get the just-set value
+            resp = read_packet(ser)
+            resp_cntl = resp[0]
+            resp_cnst = resp[1]
+            resp_val = struct.unpack('f', resp[2:6])[0]
+            
+            if resp_cntl != cntl_id or resp_cnst != const_id or abs(resp_val - to_set) > 1e-5:
+                print("Failed get/set test. Expected controller " + 
+                    str(cntl_id) + ", constant " + str(const_id) + ", value " + str(to_set))
+                print("    Received " + str(resp_cntl) + ", constant " + str(resp_cnst) + ", value " + str(resp_val))
+                passed = False
+
+    ret_status = query_received(ser)
+    return passed, ret_status
 	
 if __name__ == '__main__':
     with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
@@ -126,4 +146,5 @@ if __name__ == '__main__':
         passed, status = test_blast(ser, status, 150, 80)
         passed, status = test_bad_checksum(ser, status)
         passed, status = test_checksum(ser)
+        passed, status = test_get_set(ser)