From 2232728f6aba83ff1c1fd1729f478fad0429ed05 Mon Sep 17 00:00:00 2001
From: burneykb <burneykb@iastate.edu>
Date: Fri, 17 Feb 2017 20:29:10 -0600
Subject: [PATCH] might be ok
---
groundStation/src/backend/backend.c | 11 ++---
groundStation/src/backend/cmHandler.c | 63 ++++++++++++++++-----------
groundStation/src/backend/cmHandler.h | 8 +---
groundStation/src/backend/commands.h | 8 ++--
4 files changed, 49 insertions(+), 41 deletions(-)
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index d206bb5b2..f0fd761f4 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -694,11 +694,11 @@ static void client_recv(int fd) {
}
}
- // printf("packet = '");
- // for(int i = 0; i < (int)psize; ++i) {
- // printf(" %.2x ", packet[i]);
- // }
- // printf("'\n");
+ printf("packet = '");
+ for(int i = 0; i < (int)psize; ++i) {
+ printf(" %.2x ", packet[i]);
+ }
+ printf("'\n");
writeQuad(packet, psize);
@@ -777,6 +777,7 @@ static void quad_recv() {
switch (m.msg_type) {
case DEBUG_ID :
+ printf()
case PACKETLOG_ID :
case GETPACKETLOGS_ID :
case UPDATE_ID :
diff --git a/groundStation/src/backend/cmHandler.c b/groundStation/src/backend/cmHandler.c
index 062cae960..54a1be157 100644
--- a/groundStation/src/backend/cmHandler.c
+++ b/groundStation/src/backend/cmHandler.c
@@ -6,27 +6,21 @@ const char * respParamStrings[MAX_PARAM_COMMANDS] = {
"getrollp",
"getrolli",
"getrolld",
- "getroll",
"getpitchp",
"getpitchi",
"getpitchd",
- "getpitch",
"getyawp",
"getyawi",
"getyawd",
- "getyaw",
"getrollratep",
"getrollratei",
"getrollrated",
- "getrollrate",
"getpitchratep",
"getpitchratei",
"getpitchrated",
- "getpitchrate",
"getyawratep",
"getyawratei",
"getyawrated",
- "getyawrate",
"getlatp",
"getlati",
"getlatd",
@@ -45,27 +39,21 @@ const char * setParamStrings[MAX_PARAM_COMMANDS] = {
"setrollp",
"setrolli",
"setrolld",
- "setroll",
"setpitchp",
"setpitchi",
"setpitchd",
- "setpitch",
"setyawp",
"setyawi",
"setyawd",
- "setyaw",
"setrollratep",
"setrollratei",
"setrollrated",
- "setrollrate",
"setpitchratep",
"setpitchratei",
"setpitchrated",
- "setpitchrate",
"setyawratep",
"setyawratei",
"setyawrated",
- "setyawrate",
"setlatp",
"setlati",
"setlatd",
@@ -77,7 +65,7 @@ const char * setParamStrings[MAX_PARAM_COMMANDS] = {
"setheightp",
"setheighti",
"setheightd",
- "setheight"
+ "setheight",
};
@@ -141,34 +129,57 @@ struct controller_message * stringToCm(const char * string, struct controller_me
if (index == -1) {
return NULL;
} else {
- cm->value_id = i % MAX_CONTROL_PARAM_ID;
- if (i <= PARAM_ROLL) {
+ cm->value_id = i % MAX_CONTROL_PARAM_ID; // Default assuming not a setpoint command
+ if (i <= PARAM_ROLL_D) {
cm->id = ROLL_ID;
- } else if (i <= PARAM_PITCH) {
+ } else if (i <= PARAM_PITCH_D) {
cm->id = PITCH_ID;
- } else if (i <= PARAM_YAW) {
+ } else if (i <= PARAM_YAW_D) {
cm->id = YAW_ID;
- } else if (i <= PARAM_ROLL_RATE) {
+ } else if (i <= PARAM_ROLL_RATE_D) {
cm->id = ROLL_RATE_ID;
- } else if (i <= PARAM_PITCH_RATE) {
+ } else if (i <= PARAM_PITCH_RATE_D) {
cm->id = PITCH_RATE_ID;
- } else if (i <= PARAM_YAW_RATE) {
+ } else if (i <= PARAM_YAW_RATE_D) {
cm->id = YAW_RATE_ID;
} else if (i <= PARAM_LOCAL_X) {
-
- cm->id = LOCAL_X_ID;
+ // If setpoint command, change the cm->valueid and cm
+ if (i == PARAM_LOCAL_X) {
+ cm->id = X_SETPOINT_ID;
+ cm->value_id = 0;
+ } else {
+ // these have a slightly different value id offset
+ cm->id = LOCAL_X_ID;
+ cm->value_id = MAX_CONTROL_PARAM_ID - (PARAM_LOCAL_X - i);
+ }
} else if (i <= PARAM_LOCAL_Y) {
-
- cm->id = LOCAL_Y_ID;
+ // If setpoint command, change the cm->valueid and cm
+ if (i == PARAM_LOCAL_Y) {
+ cm->id = Y_SETPOINT_ID;
+ cm->value_id = 0;
+ } else {
+ // these have a slightly different value id offset
+ cm->id = LOCAL_Y_ID;
+ cm->value_id = MAX_CONTROL_PARAM_ID - (PARAM_LOCAL_Y - i);
+
+ }
} else if (i <= PARAM_ALT) {
-
- cm->id = ALT_ID;
+ // If setpoint command, change the cm->valueid and cm
+ if (i == PARAM_ALT) {
+ cm->id = ALT_SETPOINT_ID;
+ cm->value_id = 0;
+ } else {
+ // these have a slightly different value id offset
+ cm->id = ALT_ID;
+ cm->value_id = MAX_CONTROL_PARAM_ID - (PARAM_ALT - i);
+
+ }
}
}
return cm;
diff --git a/groundStation/src/backend/cmHandler.h b/groundStation/src/backend/cmHandler.h
index ee51ca658..ff749328e 100644
--- a/groundStation/src/backend/cmHandler.h
+++ b/groundStation/src/backend/cmHandler.h
@@ -7,31 +7,25 @@ enum paramIndices {
PARAM_ROLL_P ,
PARAM_ROLL_I ,
PARAM_ROLL_D ,
- PARAM_ROLL ,
PARAM_PITCH_P ,
PARAM_PITCH_I ,
PARAM_PITCH_D ,
- PARAM_PITCH ,
PARAM_YAW_P ,
PARAM_YAW_I ,
PARAM_YAW_D ,
- PARAM_YAW ,
PARAM_ROLL_RATE_P,
PARAM_ROLL_RATE_I,
PARAM_ROLL_RATE_D,
- PARAM_ROLL_RATE,
PARAM_PITCH_RATE_P,
PARAM_PITCH_RATE_I,
PARAM_PITCH_RATE_D,
- PARAM_PITCH_RATE,
PARAM_YAW_RATE_P,
PARAM_YAW_RATE_I,
PARAM_YAW_RATE_D,
- PARAM_YAW_RATE,
PARAM_LOCAL_X_P ,
PARAM_LOCAL_X_I ,
PARAM_LOCAL_X_D ,
- PARAM_LOCAL_X ,
+ PARAM_LOCAL_X ,
PARAM_LOCAL_Y_P ,
PARAM_LOCAL_Y_I ,
PARAM_LOCAL_Y_D ,
diff --git a/groundStation/src/backend/commands.h b/groundStation/src/backend/commands.h
index 578b20f7b..6dda7213d 100644
--- a/groundStation/src/backend/commands.h
+++ b/groundStation/src/backend/commands.h
@@ -60,7 +60,10 @@ enum ControllerID{
LOCAL_X_ID, // 06 - Local X PID
LOCAL_Y_ID, // 07 - Local Y PID
ALT_ID, // 08 - Altitude PID
- MAX_CONTROLLER_ID, // 09 - Just used to keep track of the size
+ X_SETPOINT_ID, // 09 - X Setpoint
+ Y_SETPOINT_ID, // 10 - Y Setpoint
+ ALT_SETPOINT_ID, // 11 - Z Setpoint
+ MAX_CONTROLLER_ID // 12 - Just used to keep track of the size
};
/*
@@ -70,8 +73,7 @@ enum ControlParamID{
KP_ID, // 00 - P constant
KI_ID, // 01 - I constant
KD_ID, // 02 - D constant
- SP_ID, // 03 - Setpoint value
- MAX_CONTROL_PARAM_ID, // 04 - Just used to keep track of the size
+ MAX_CONTROL_PARAM_ID, // 03 - Just used to keep track of the size
};
/*
--
GitLab