From 19fedcae75b4290915c53448785e391fe4ad9157 Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <dawehr@iastate.edu>
Date: Sat, 28 Jan 2017 20:28:50 -0600
Subject: [PATCH] - Made continuity changes throughout code base to match the
new commands.c/h - removed every use of the "subtype" term - made the "type"
term 16 bits - made sure that every reference to a message type is 16 bits
and used correctly - simplified commands enum names - implemented setcontrol
callback function that switch on the controller and P, I, D, setpoint value
to be set and then set them - partly implemented getcontrol callback function
- NOTE: setpoint is a double. this causes some problems.. - TODO: add
setpoints for rate controllers - TODO: figure out what message ID I should
send back when I get a getpid command
---
groundStation/src/backend/commands.c | 48 ++--
groundStation/src/backend/commands.h | 66 +++--
quad/sw/modular_quad_pid/src/callbacks.c | 258 ++++++++++++++++--
.../src/initialize_components.c | 2 +-
quad/sw/modular_quad_pid/src/log_data.c | 4 +-
quad/sw/modular_quad_pid/src/sensor.c | 5 +-
quad/sw/modular_quad_pid/src/type_def.h | 4 +-
.../sw/modular_quad_pid/test/test_uart_buff.c | 4 +-
8 files changed, 306 insertions(+), 85 deletions(-)
diff --git a/groundStation/src/backend/commands.c b/groundStation/src/backend/commands.c
index 59c09ca4c..fe8151978 100644
--- a/groundStation/src/backend/commands.c
+++ b/groundStation/src/backend/commands.c
@@ -30,7 +30,7 @@
*
* EXTENDING COMMANDS.C
*
- * To extend this file, simply add the new subtypes (typically
+ * To extend this file, simply add the new type (typically
* a Setter, Getter, and Response) and create weak aliases below.
*
* Ensure the Quad and GroundStation always maintain this file in sync!
@@ -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_setval __attribute__((weak, alias("cb_default")));
-command_cb cb_getval __attribute__((weak, alias("cb_default")));
-command_cb cb_respval __attribute__((weak, alias("cb_default")));
+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 structure.
@@ -69,12 +69,12 @@ command_cb cb_respval __attribute__((weak, alias("cb_default")));
* There is one callback function pointer associated with each
* element in this struct array.
*/
-struct MessageType MessageTypes[MAX_TYPE] =
+struct MessageType MessageTypes[MAX_TYPE_ID] =
{
// DEBUG
{
// ID
- DEBUG_TYPE_ID,
+ DEBUG_ID,
// Command text
"debug",
// Type of the command data
@@ -85,7 +85,7 @@ struct MessageType MessageTypes[MAX_TYPE] =
// PACKETLOG
{
// ID
- PACKETLOG_TYPE_ID,
+ PACKETLOG_ID,
// Command text
"packetlog",
// Type of the command data
@@ -96,7 +96,7 @@ struct MessageType MessageTypes[MAX_TYPE] =
// GETPACKETLOGS
{
// ID
- GETPACKETLOGS_TYPE_ID,
+ GETPACKETLOGS_ID,
// Command text
"getpacketlogs",
// Type of the command data
@@ -107,7 +107,7 @@ struct MessageType MessageTypes[MAX_TYPE] =
// UPDATE
{
// ID
- UPDATE_TYPE_ID,
+ UPDATE_ID,
// Command text
"update",
// Type of the command data
@@ -118,7 +118,7 @@ struct MessageType MessageTypes[MAX_TYPE] =
// BEGINUPDATE
{
// ID
- BEGINUPDATE_TYPE_ID,
+ BEGINUPDATE_ID,
// Command text
"beginupdate",
// Type of the command data
@@ -129,7 +129,7 @@ struct MessageType MessageTypes[MAX_TYPE] =
// LOG
{
// ID
- LOG_TYPE_ID,
+ LOG_ID,
// Command text
"log",
// Type of the command data
@@ -140,7 +140,7 @@ struct MessageType MessageTypes[MAX_TYPE] =
// RESPONSE
{
// ID
- RESPONSE_TYPE_ID,
+ RESPONSE_ID,
// Command text
"response",
// Type of the command data
@@ -148,38 +148,38 @@ struct MessageType MessageTypes[MAX_TYPE] =
// Function pointer
&cb_response
},
- // SETVAL
+ // SETCONTROL
{
// ID
- SETVAL_TYPE_ID,
+ SETCONTROL_ID,
// Command text
- "setval",
+ "setcontrol",
// Type of the command data
floatType,
// Function pointer
- &cb_setval
+ &cb_setcontrol
},
- // GETVAL
+ // GETCONTROL
{
// ID
- GETVAL_TYPE_ID,
+ GETCONTROL_ID,
// Command text
- "getval",
+ "getcontrol",
// Type of the command data
floatType,
// Function pointer
- &cb_getval
+ &cb_getcontrol
},
- // RESPVAL
+ // RESPCONTROL
{
// ID
- RESPVAL_TYPE_ID,
+ RESPCONTROL_ID,
// Command text
- "respval",
+ "respcontrol",
// Type of the command data
floatType,
// Function pointer
- &cb_respval
+ &cb_respcontrol
}
};
diff --git a/groundStation/src/backend/commands.h b/groundStation/src/backend/commands.h
index 24d61f0a2..ecbae2c04 100644
--- a/groundStation/src/backend/commands.h
+++ b/groundStation/src/backend/commands.h
@@ -20,9 +20,9 @@ enum Message{
*/
enum DataType
{
- floatType,
- intType,
- stringType
+ floatType, // 00
+ intType, // 01
+ stringType // 02
};
/*
@@ -31,44 +31,42 @@ enum DataType
* DO NOT change this enum or you will break backwards compatibility.
*/
enum MessageTypeID{
- DEBUG_TYPE_ID, //
- PACKETLOG_TYPE_ID, //
- GETPACKETLOGS_TYPE_ID, //
- UPDATE_TYPE_ID, //
- BEGINUPDATE_TYPE_ID, //
- LOG_TYPE_ID, //
- RESPONSE_TYPE_ID, //
- SETVAL_TYPE_ID, // setting values. Example: PID constants
- GETVAL_TYPE_ID, // getting values. Example: PID constants
- RESPVAL_TYPE_ID, // responding with values. Example: PID constants
- // This last type is just used to keep track of the size
- MAX_TYPE
+ DEBUG_ID, // 00
+ PACKETLOG_ID, // 01
+ GETPACKETLOGS_ID, // 02
+ UPDATE_ID, // 03
+ 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
+ MAX_TYPE_ID // 10 - Just used to keep track of the size
};
/*
- * Value types used to know which value the command is referencing
+ * Controller ID used to know which controller the message is referencing
*/
-enum ValueType{
- // PID controllers. Each one has a P, I, D, setpoint value option
- ROLL,
- PITCH,
- YAW,
- ROLL_RATE,
- PITCH_RATE,
- YAW_RATE,
- LOCAL_X,
- LOCAL_Y,
- ALT,
+enum ControllerID{
+ ROLL_ID, // 00 - Roll PID
+ PITCH_ID, // 01 - Pitch PID
+ YAW_ID, // 02 - Yaw PID
+ ROLL_RATE_ID, // 03 - Roll rate PID
+ PITCH_RATE_ID, // 04 - Pitch rate PID
+ YAW_RATE_ID, // 05 - Yaw rate PID
+ LOCAL_X_ID, // 06 - Local X PID
+ LOCAL_Y_ID, // 07 - Local Y PID
+ ALT_ID, // 08 - Altitude PID
};
/*
- * Enumeration of PID constants
+ * Enumeration of controller values
*/
-enum PIDType{
- KP, // P constant
- KI, // I constant
- KD, // D constant
- SP, // Setpoint value
+enum ControllerValueID{
+ KP_ID, // 00 - P constant
+ KI_ID, // 01 - I constant
+ KD_ID, // 02 - D constant
+ SP_ID, // 03 - Setpoint value
};
/*
@@ -90,7 +88,7 @@ struct MessageType{
};
/* Defined in commands.c */
-extern struct MessageType MessageTypes[MAX_TYPE];
+extern struct MessageType MessageTypes[MAX_TYPE_ID];
/* Legacy functions - no idea what uses these. Please do not delete. */
float getFloat(unsigned char* str, int pos);
diff --git a/quad/sw/modular_quad_pid/src/callbacks.c b/quad/sw/modular_quad_pid/src/callbacks.c
index b1519926a..52705f5b4 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.c
+++ b/quad/sw/modular_quad_pid/src/callbacks.c
@@ -39,7 +39,7 @@ int cb_getpacketlogs(modular_structs_t* structs) {
// Message logging number of messages received and size of payload received
int length = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);
- send_data(LOG_TYPE_ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ send_data(LOG_ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
@@ -93,32 +93,256 @@ int cb_beginupdate(modular_structs_t *structs) {
/* Callbacks for configuration */
/**
- * Handles a command to set a value on the quad.
+ * Handles a command to set a controller value on the quad.
*
- * This function handles setting things like:
- * - PID constants
- * - set points
- * - other things in the future, etc.
+ * NOTE: expects the uart buff to have some data in the following format:
+ * |------------------------------------------------------|
+ * | data index|| 0 | 1 | 3 - 6 |
+ * |------------------------------------------------------|
+ * | param|| control ID | ctrl val ID | float val |
+ * |------------------------------------------------------|
+ * | bytes|| 1 | 1 | 4 |
+ * |------------------------------------------------------|
*/
-int cb_setval(modular_structs_t *structs)
+int cb_setcontrol(modular_structs_t *structs)
{
- //structs->parameter_struct.roll_angle_pid.Kp = uart_buff_data_get_float(0);
+ // 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);
+
+ // TODO check if the setpoints really need to be doubles
+ // TODO add rate setpoints
+ // Switch case on the controller ID
+ switch(controller_id)
+ {
+ case ROLL_ID:
+ // switch on the value ID
+ switch(controller_value_id)
+ {
+ case KP_ID:
+ structs->parameter_struct.roll_angle_pid.Kp = controller_value;
+ break;
+ case KI_ID:
+ structs->parameter_struct.roll_angle_pid.Ki = controller_value;
+ break;
+ case KD_ID:
+ structs->parameter_struct.roll_angle_pid.Kd = controller_value;
+ break;
+ case SP_ID:
+ structs->setpoint_struct.desiredQuadPosition.roll = controller_value;
+ break;
+ }
+ break;
+ case PITCH_ID:
+ // switch on the value ID
+ switch(controller_value_id)
+ {
+ case KP_ID:
+ structs->parameter_struct.pitch_angle_pid.Kp = controller_value;
+ break;
+ case KI_ID:
+ structs->parameter_struct.pitch_angle_pid.Ki = controller_value;
+ break;
+ case KD_ID:
+ structs->parameter_struct.pitch_angle_pid.Kd = controller_value;
+ break;
+ case SP_ID:
+ structs->setpoint_struct.desiredQuadPosition.pitch = controller_value;
+ break;
+ }
+ break;
+ case YAW_ID:
+ // switch on the value ID
+ switch(controller_value_id)
+ {
+ case KP_ID:
+ structs->parameter_struct.yaw_angle_pid.Kp = controller_value;
+ break;
+ case KI_ID:
+ structs->parameter_struct.yaw_angle_pid.Ki = controller_value;
+ break;
+ case KD_ID:
+ structs->parameter_struct.yaw_angle_pid.Kd = controller_value;
+ break;
+ case SP_ID:
+ structs->setpoint_struct.desiredQuadPosition.yaw = controller_value;
+ break;
+ }
+ break;
+ case ROLL_RATE_ID:
+ // switch on the value ID
+ switch(controller_value_id)
+ {
+ case KP_ID:
+ structs->parameter_struct.roll_ang_vel_pid.Kp = controller_value;
+ break;
+ case KI_ID:
+ structs->parameter_struct.roll_ang_vel_pid.Ki = controller_value;
+ break;
+ case KD_ID:
+ structs->parameter_struct.roll_ang_vel_pid.Kd = controller_value;
+ break;
+ case SP_ID:
+ // do nothing for now because there is no set point for this conroller
+ break;
+ }
+ break;
+ case PITCH_RATE_ID:
+ // switch on the value ID
+ switch(controller_value_id)
+ {
+ case KP_ID:
+ structs->parameter_struct.pitch_ang_vel_pid.Kp = controller_value;
+ break;
+ case KI_ID:
+ structs->parameter_struct.pitch_ang_vel_pid.Ki = controller_value;
+ break;
+ case KD_ID:
+ structs->parameter_struct.pitch_ang_vel_pid.Kd = controller_value;
+ break;
+ case SP_ID:
+ // do nothing for now because there is no set point for this conroller
+ break;
+ }
+ break;
+ case YAW_RATE_ID:
+ // switch on the value ID
+ switch(controller_value_id)
+ {
+ case KP_ID:
+ structs->parameter_struct.yaw_ang_vel_pid.Kp = controller_value;
+ break;
+ case KI_ID:
+ structs->parameter_struct.yaw_ang_vel_pid.Ki = controller_value;
+ break;
+ case KD_ID:
+ structs->parameter_struct.yaw_ang_vel_pid.Kd = controller_value;
+ break;
+ case SP_ID:
+ // do nothing for now because there is no set point for this conroller
+ break;
+ }
+ break;
+ case LOCAL_X_ID:
+ // switch on the value ID
+ switch(controller_value_id)
+ {
+ case KP_ID:
+ structs->parameter_struct.local_x_pid.Kp = controller_value;
+ break;
+ case KI_ID:
+ structs->parameter_struct.local_x_pid.Ki = controller_value;
+ break;
+ case KD_ID:
+ structs->parameter_struct.local_x_pid.Kd = controller_value;
+ break;
+ case SP_ID:
+ structs->setpoint_struct.desiredQuadPosition.x_pos = controller_value;
+ break;
+ }
+ break;
+ case LOCAL_Y_ID:
+ // switch on the value ID
+ switch(controller_value_id)
+ {
+ case KP_ID:
+ structs->parameter_struct.local_y_pid.Kp = controller_value;
+ break;
+ case KI_ID:
+ structs->parameter_struct.local_y_pid.Ki = controller_value;
+ break;
+ case KD_ID:
+ structs->parameter_struct.local_y_pid.Kd = controller_value;
+ break;
+ case SP_ID:
+ structs->setpoint_struct.desiredQuadPosition.y_pos = controller_value;
+ break;
+ }
+ break;
+ case ALT_ID:
+ // switch on the value ID
+ switch(controller_value_id)
+ {
+ case KP_ID:
+ structs->parameter_struct.alt_pid.Kp = controller_value;
+ break;
+ case KI_ID:
+ structs->parameter_struct.alt_pid.Ki = controller_value;
+ break;
+ case KD_ID:
+ structs->parameter_struct.alt_pid.Kd = controller_value;
+ break;
+ case SP_ID:
+ structs->setpoint_struct.desiredQuadPosition.alt_pos = controller_value;
+ break;
+ }
+ break;
+ }
+
return 0;
}
/**
- * Handles a command to get a value from the quad.
+ * Handles a command to get a controller value from the quad.
*
- * Used to get basically anything that setval can set.
- * NOTE: This function sends a response command with the desired data
- * back to the ground station.
+ * NOTE: expects the uart buff to have some data in the following format:
+ * |----------------------------------------|
+ * | data index|| 0 | 1 |
+ * |----------------------------------------|
+ * | param|| control ID | ctrl val ID |
+ * |----------------------------------------|
+ * | bytes|| 1 | 1 |
+ * |----------------------------------------|
*/
-int cb_getval(modular_structs_t* structs) {
- char buf[255];
+int cb_getcontrol(modular_structs_t* structs)
+{
+ // 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);
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.yaw_angle_pid.Kp);
+ // TODO check if the setpoints really need to be doubles
+ // TODO add rate setpoints
+ // TODO figure out what the message ID should be
+ // Switch case on the controller ID
+ switch(controller_id)
+ {
+ case ROLL_ID:
+ // switch on the value ID
+ switch(controller_value_id)
+ {
+ case KP_ID:
+ float controller_value = structs->parameter_struct.roll_angle_pid.Kp;
+ send_data(RESPCONTROL_ID, 0, ((char *) &controller_value), sizeof(controller_value));
+ break;
+ case KI_ID:
+ structs->parameter_struct.roll_angle_pid.Ki = controller_value;
+ break;
+ case KD_ID:
+ structs->parameter_struct.roll_angle_pid.Kd = controller_value;
+ break;
+ case SP_ID:
+ structs->setpoint_struct.desiredQuadPosition.roll = controller_value;
+ break;
+ }
+ break;
+ case PITCH_ID:
+ // TODO
+ case YAW_ID:
+ // TODO
+ case ROLL_RATE_ID:
+ // TODO
+ case PITCH_RATE_ID:
+ // TODO
+ case YAW_RATE_ID:
+ // TODO
+ case LOCAL_X_ID:
+ // TODO
+ case LOCAL_Y_ID:
+ // TODO
+ case ALT_ID:
+ // TODO
+ }
- send_data(GETVAL_TYPE_ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.c b/quad/sw/modular_quad_pid/src/initialize_components.c
index 740268d6b..b56ac6323 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.c
+++ b/quad/sw/modular_quad_pid/src/initialize_components.c
@@ -33,7 +33,7 @@ int protection_loops(modular_structs_t *structs)
length = length >= sizeof(buf) ? 255 : length;
while (!structs->user_input_struct.receivedBeginUpdate) {
- //send_data(MessageTypes[4].ID, MessageTypes[4].subtypes[1].ID, 0, buf, length);
+ //send_data(BEGINUPDATE_TYPE_ID, 0, buf, length);
process_received(structs);
usleep(10000);
}
diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c
index 9bbf7cd18..2ab248cdd 100644
--- a/quad/sw/modular_quad_pid/src/log_data.c
+++ b/quad/sw/modular_quad_pid/src/log_data.c
@@ -147,7 +147,7 @@ void printLogging(){
strcat(buf,header);
strcat(buf,units);
- send_data(LOG_TYPE_ID, 0, buf, strlen(buf) + 1);
+ send_data(LOG_ID, 0, buf, strlen(buf) + 1);
//uart0_sendBytes(buf, strlen(buf));
//usleep(100000);
@@ -155,7 +155,7 @@ void printLogging(){
/* print & send log data */
for(i = 0; i < arrayIndex; i++){
char* logLine = format(logArray[i]);
- send_data(LOG_TYPE_ID, 0, logLine, strlen(logLine) + 1);
+ send_data(LOG_ID, 0, logLine, strlen(logLine) + 1);
free(logLine);
}
}
diff --git a/quad/sw/modular_quad_pid/src/sensor.c b/quad/sw/modular_quad_pid/src/sensor.c
index 86d1aebd4..9f24a3b76 100644
--- a/quad/sw/modular_quad_pid/src/sensor.c
+++ b/quad/sw/modular_quad_pid/src/sensor.c
@@ -38,12 +38,11 @@ int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t
// metadata_t metadata =
// {
// BEGIN_CHAR,
-// MessageTypes[5].ID,
-// MessageTypes[5].subtypes[1].ID,
+// RESPONSE_TYPE_ID,
// 0,
// (strlen(buf) + 1)
// };
-// formatPacket(&metadata, buf, &responsePacket);
+// formatPacket(&metadata, buf, MessageTypes[RESPONSE_TYPE_ID].functionPtr);
//
// // Send each byte of the packet individually
// int i;
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index 034f6b770..279d3d413 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -22,9 +22,9 @@ enum flight_mode{
//----------------------------------------------------------------------------------------------
// index|| 0 | 1 | 2 | 3 & 4 | 5 & 6 | 7+ | end |
//---------------------------------------------------------------------------------------------|
-// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
+// msg param|| beg char | msg type | msg id | data len (bytes) | data | checksum |
//-------------------------------------------------------------------------------------------- |
-// bytes|| 1 | 1 | 1 | 2 | 2 | var | 1 |
+// bytes|| 1 | 2 | 2 | 2 | var | 1 |
//----------------------------------------------------------------------------------------------
typedef struct {
char begin_char;
diff --git a/quad/sw/modular_quad_pid/test/test_uart_buff.c b/quad/sw/modular_quad_pid/test/test_uart_buff.c
index 29b127bbd..72eb683ed 100644
--- a/quad/sw/modular_quad_pid/test/test_uart_buff.c
+++ b/quad/sw/modular_quad_pid/test/test_uart_buff.c
@@ -50,10 +50,10 @@ int failed(char *msg) {
return 1;
}
-void add_packet(unsigned char type, unsigned char subtype, unsigned short id, unsigned short length, unsigned char *data) {
+void add_packet(u16 type, unsigned short id, unsigned short length, unsigned char *data) {
uart_buff_add_u8(0xBE);
uart_buff_add_u8(type);
- uart_buff_add_u8(subtype);
+ uart_buff_add_u8(type >> 8);
uart_buff_add_u8(id);
uart_buff_add_u8(id >> 8);
uart_buff_add_u8(length);
--
GitLab