diff --git a/groundStation/src/backend/commands.c b/groundStation/src/backend/commands.c
index ab89e699ad20c776da4ae2982e5a2faff6e271f4..18025ee340fd1e4238e2b657d6eeb27af3e28a81 100644
--- a/groundStation/src/backend/commands.c
+++ b/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
}
};
diff --git a/groundStation/src/backend/commands.h b/groundStation/src/backend/commands.h
index d28c91ec1b65ce723fa7cbc303d2bf9fdfc851cc..578b20f7b7fe20b8568905eceb2b37df6ef83cc9 100644
--- a/groundStation/src/backend/commands.h
+++ b/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
};
/*
diff --git a/quad/scripts/stress_tests.py b/quad/scripts/stress_tests.py
index 7b31502bf1ea436ce0757c5f9883fcad29fc7d15..801f1713fa9beadbe9c7ebb45322d0e6eadec0bd 100755
--- a/quad/scripts/stress_tests.py
+++ b/quad/scripts/stress_tests.py
@@ -1,15 +1,15 @@
#!/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()
diff --git a/quad/sw/modular_quad_pid/src/callbacks.c b/quad/sw/modular_quad_pid/src/callbacks.c
index 0a166ca4fd27a9ebafdb5ecbbef20d963a6f72a2..e34fe1db454a6be43edeb818e650d87c50705fdb 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.c
+++ b/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], ¶m_val, sizeof(param_val));
+
+ // Send the response
+ send_data(RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
+
return 0;
}