Updated stress_tests.py to work with new commands

quad/scripts/stress_tests.py

 #!/usr/local/bin/python3.6
 
 import sys
+import random
 from time import sleep
 
 import serial
 
-def create_msg(main_type, subtype, msg_id, data):
+def create_msg(msg_type, msg_id, data):
     msg = bytes()
     msg += b'\xBE'
-    msg += main_type.to_bytes(1, 'little')
-    msg += subtype.to_bytes(1, 'little')
+    msg += msg_type.to_bytes(2, 'little')
     msg += msg_id.to_bytes(2, 'little')
     msg += len(data).to_bytes(2, 'little')
     msg += data
@@ -22,7 +22,7 @@ def create_msg(main_type, subtype, msg_id, data):
 
 def create_test_packet(size=8):
     data = bytes((i % 256 for i in range(size)))
-    return create_msg(0, 1, 0, data)
+    return create_msg(1, 0, data)
 
 def read_packet(ser, raw=False):
     header = ser.read(7)
@@ -37,7 +37,7 @@ def read_packet(ser, raw=False):
 
 def query_received(ser):
     # Send request
-    query_msg = create_msg(0, 2, 0, b'')
+    query_msg = create_msg(2, 0, b'')
     ser.write(query_msg)
     ser.flush()
     sleep(0.1)
@@ -107,6 +107,13 @@ def test_bad_checksum(ser, cur_status, size=30):
     ser.flush()
     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()
+	
 if __name__ == '__main__':
     with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
         ser.reset_input_buffer()