Merge remote-tracking branch 'origin/commands-dev' into commands-dev-backend

groundStation/src/backend/commands.c

@@ -57,9 +57,9 @@ command_cb cb_log __attribute__((weak, alias("cb_default")));
 command_cb cb_response __attribute__((weak, alias("cb_default")));
 
 /* Callbacks for configuration */
-command_cb cb_setcontrol __attribute__((weak, alias("cb_default")));
-command_cb cb_getcontrol __attribute__((weak, alias("cb_default")));
-command_cb cb_respcontrol __attribute__((weak, alias("cb_default")));
+command_cb cb_setparam __attribute__((weak, alias("cb_default")));
+command_cb cb_getparam __attribute__((weak, alias("cb_default")));
+command_cb cb_respparam __attribute__((weak, alias("cb_default")));
 
 /*
  * Command structure.
@@ -137,32 +137,32 @@ struct MessageType MessageTypes[MAX_TYPE_ID] =
 		// Function pointer
 		&cb_response
 	},
-	// SETCONTROL
+	// SETPARAM
 	{
 		// Command text
-		"setcontrol",
+		"setparam",
 		// Type of the command data
 		floatType,
 		// Function pointer
-		&cb_setcontrol
+		&cb_setparam
 	},
-	// GETCONTROL
+	// GETPARAM
 	{
 		// Command text
-		"getcontrol",
+		"getparam",
 		// Type of the command data
 		floatType,
 		// Function pointer
-		&cb_getcontrol
+		&cb_getparam
 	},
-	// RESPCONTROL
+	// RESPPARAM
 	{
 		// Command text
-		"respcontrol",
+		"respparam",
 		// Type of the command data
 		floatType,
 		// Function pointer
-		&cb_respcontrol
+		&cb_respparam
 	}
 
 };

groundStation/src/backend/commands.h

@@ -41,9 +41,9 @@ enum MessageTypeID{
 	BEGINUPDATE_ID,       // 04
 	LOG_ID,               // 05
 	RESPONSE_ID,          // 06
-	SETCONTROL_ID,        // 07 - Setting controller values. Example: PID constants
-	GETCONTROL_ID,        // 08 - Getting controller values. Example: PID constants
-	RESPCONTROL_ID,       // 09 - Responding with controller values. Example: PID constants
+	SETPARAM_ID,          // 07 - Setting controller parameters. Example: PID constants
+	GETPARAM_ID,          // 08 - Getting controller parameters. Example: PID constants
+	RESPPARAM_ID,         // 09 - Responding with controller parameters. Example: PID constants
 	MAX_TYPE_ID           // 10 - Just used to keep track of the size
 };
 
@@ -64,14 +64,14 @@ enum ControllerID{
 };
 
 /*
- * Enumeration of controller values
+ * Enumeration of controller parameters
  */
-enum ControllerValueID{
+enum ControlParamID{
 	KP_ID,                 // 00 - P constant
 	KI_ID,                 // 01 - I constant
 	KD_ID,                 // 02 - D constant
 	SP_ID,                 // 03 - Setpoint value
-	MAX_CONTROL_VAL_ID,    // 04 - Just used to keep track of the size
+	MAX_CONTROL_PARAM_ID,  // 04 - Just used to keep track of the size
 };
 
 /*

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()

quad/sw/modular_quad_pid/src/callbacks.c

@@ -93,68 +93,70 @@ int cb_beginupdate(modular_structs_t *structs) {
 /* Callbacks for configuration */
 
 /**
-  * Handles a command to set a controller value on the quad.
+  * Handles a command to set a controller parameter on the quad.
   *
   * NOTE:
   * Expects the uart buff to have data in the following format:
   * |--------------------------------------------------------|
   * |  data index ||      0      |      1      |    2 - 5    |
   * |--------------------------------------------------------|
-  * |       param ||  control ID | ctrl val ID |  float val  |
+  * |   parameter ||  control ID | ctrl parmID |  param val  |
   * |--------------------------------------------------------|
   * |       bytes ||      1      |      1      |      4      |
   * |--------------------------------------------------------|
   * 
   * Does not send anything in response.
   */
-int cb_setcontrol(modular_structs_t *structs)
+int cb_setparam(modular_structs_t *structs)
 {
 	// Get some of the meta data
-	u16 data_len = uart_buff_get_u16(6);
+	u16 data_len = uart_buff_data_length();
 	// Check if the data length is correct
-	if (data_len == 6)
+	if (data_len != 6)
 	{
-		// Get the controller ID, value ID, float value
-		u8 controller_id = uart_buff_data_get_u8(0);
-		u8 controller_value_id = uart_buff_data_get_u8(1);
-		float controller_value = uart_buff_data_get_float(3);
-
-		// Check to make sure the IDs are in bounds
-		if (controller_id < MAX_CONTROLLER_ID &&
-			controller_value_id < MAX_CONTROL_VAL_ID)
-		{
-			// Set the controller_value into the controller by controller_id, controller_value_id
-			switch(controller_value_id)
-			{
-				case KP_ID:
-					structs->parameter_struct.pid_controllers[controller_id].Kp = controller_value;
-					break;
-				case KI_ID:
-					structs->parameter_struct.pid_controllers[controller_id].Ki = controller_value;
-					break;
-				case KD_ID:
-					structs->parameter_struct.pid_controllers[controller_id].Kd = controller_value;
-					break;
-				case SP_ID:
-					structs->parameter_struct.pid_controllers[controller_id].setpoint = controller_value;
-					break;
-			}
-		}
+		return -1;
+	}
+
+	// Get the controller ID, parameter ID, parameter value
+	u8 controller_id = uart_buff_data_get_u8(0);
+	u8 param_id = uart_buff_data_get_u8(1);
+	float param_val = uart_buff_data_get_float(3);
+	// Check to make sure the IDs are in bounds
+	if (controller_id >= MAX_CONTROLLER_ID ||
+		param_id >= MAX_CONTROL_PARAM_ID)
+	{
+		return -1;
+	}
 
+	// Set the param_val into the controller by controller_id, param_id
+	switch(param_id)
+	{
+		case KP_ID:
+			structs->parameter_struct.pid_controllers[controller_id].Kp = param_val;
+			break;
+		case KI_ID:
+			structs->parameter_struct.pid_controllers[controller_id].Ki = param_val;
+			break;
+		case KD_ID:
+			structs->parameter_struct.pid_controllers[controller_id].Kd = param_val;
+			break;
+		case SP_ID:
+			structs->parameter_struct.pid_controllers[controller_id].setpoint = param_val;
+			break;
 	}
 
 	return 0;
 }
 
 /**
-  * Handles a command to get a controller value from the quad.
+  * Handles a command to get a controller parameter from the quad.
   *
   * NOTE:
   * Expects the uart buff to have data in the following format:
   * |------------------------------------------|
   * |  data index ||      0      |      1      |
   * |------------------------------------------|
-  * |       param ||  control ID | ctrl val ID |
+  * |   parameter ||  control ID | ctrl parmID |
   * |------------------------------------------|
   * |       bytes ||      1      |      1      |
   * |------------------------------------------|
@@ -165,63 +167,64 @@ int cb_setcontrol(modular_structs_t *structs)
   * |--------------------------------------------------------|
   * |  data index ||      0      |      1      |    2 - 5    |
   * |--------------------------------------------------------|
-  * |       param ||  control ID | ctrl val ID |  float val  |
+  * |   parameter ||  control ID | ctrl parmID |  param val  |
   * |--------------------------------------------------------|
   * |       bytes ||      1      |      1      |      4      |
   * |--------------------------------------------------------|
   */
-int cb_getcontrol(modular_structs_t* structs)
+int cb_getparam(modular_structs_t* structs)
 {
 	// Get some of the meta data
-	u16 data_len = uart_buff_get_u16(6);
+	u16 data_len = uart_buff_data_length();
 	u16 msg_id = uart_buff_get_u16(3);
 	// Check if the data length is correct
-	if (data_len == 2)
+	if (data_len != 2)
 	{
-		// Get the controller ID, value ID
-		u8 controller_id = uart_buff_data_get_u8(0);
-		u8 controller_value_id = uart_buff_data_get_u8(1);
-
-		// Check to make sure the IDs are in bounds
-		if (controller_id < MAX_CONTROLLER_ID &&
-			controller_value_id < MAX_CONTROL_VAL_ID)
-		{
-			// Make the variable to send
-			float controller_value;
-			// Set the controller_value equal to the controller value stored in the controller by
-			// controllerid, controller_value_id
-			switch(controller_value_id)
-			{
-				case KP_ID:
-					controller_value = structs->parameter_struct.pid_controllers[controller_id].Kp;
-					break;
-				case KI_ID:
-					controller_value = structs->parameter_struct.pid_controllers[controller_id].Ki;
-					break;
-				case KD_ID:
-					controller_value = structs->parameter_struct.pid_controllers[controller_id].Kd;
-					break;
-				case SP_ID:
-					controller_value = structs->parameter_struct.pid_controllers[controller_id].setpoint;
-					break;
-			}
-			
-			// Format the response data
-			char resp_data[6];
-
-			// Controller ID
-			resp_data[0] = controller_id;
-			// Controller value ID
-			resp_data[1] = controller_value_id;
-			// Controller value (4 byte float)
-			// TODO set a strict byte ordering for communication between the ground station and the quad
-			memcpy(&resp_data[2], &controller_value, sizeof(controller_value));
-			
-			// Send the data
-			send_data(RESPCONTROL_ID, msg_id, resp_data, sizeof(resp_data));
-		}
+		return -1;
+	}
 
+	// Get the controller ID, parameter ID
+	u8 controller_id = uart_buff_data_get_u8(0);
+	u8 param_id = uart_buff_data_get_u8(1);
+	// Check to make sure the IDs are in bounds
+	if (controller_id >= MAX_CONTROLLER_ID ||
+		param_id >= MAX_CONTROL_PARAM_ID)
+	{
+		return -1;
 	}
 
+	// Make the variable to send
+	float param_val;
+	// Set the param_val equal to the parameter value stored in the controller by
+	// controller_id, param_id
+	switch(param_id)
+	{
+		case KP_ID:
+			param_val = structs->parameter_struct.pid_controllers[controller_id].Kp;
+			break;
+		case KI_ID:
+			param_val = structs->parameter_struct.pid_controllers[controller_id].Ki;
+			break;
+		case KD_ID:
+			param_val = structs->parameter_struct.pid_controllers[controller_id].Kd;
+			break;
+		case SP_ID:
+			param_val = structs->parameter_struct.pid_controllers[controller_id].setpoint;
+			break;
+	}
+
+	// Format the response data
+	char resp_data[6];
+	// Controller ID
+	resp_data[0] = controller_id;
+	// Parameter ID
+	resp_data[1] = param_id;
+	// Parameter value (4 byte float)
+	// TODO set a strict byte ordering for communication between the ground station and the quad
+	memcpy(&resp_data[2], &param_val, sizeof(param_val));
+
+	// Send the response
+	send_data(RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
+
 	return 0;
 }