From 159e70eb3179282a1e7a519193373cbd0dc9435a Mon Sep 17 00:00:00 2001
From: burneykb <burneykb@iastate.edu>
Date: Thu, 23 Feb 2017 11:08:46 -0600
Subject: [PATCH] Working with all commands + controller network
---
groundStation/scripts/andy_params.txt | 27 +++++++++++++++++++++++++
groundStation/scripts/setpid_batch.sh | 2 +-
groundStation/scripts/tuning.txt | 6 ++++++
groundStation/src/backend/backend.c | 14 ++++---------
groundStation/src/backend/cmHandler.c | 10 ++++-----
groundStation/src/backend/commands.h | 10 ++++-----
groundStation/src/cli/cli_monitor.c | 2 +-
groundStation/src/cli/cli_setpid.c | 4 ++--
groundStation/src/frontend/pid_common.h | 2 +-
9 files changed, 52 insertions(+), 25 deletions(-)
create mode 100644 groundStation/scripts/andy_params.txt
create mode 100644 groundStation/scripts/tuning.txt
diff --git a/groundStation/scripts/andy_params.txt b/groundStation/scripts/andy_params.txt
new file mode 100644
index 000000000..0cffd21f3
--- /dev/null
+++ b/groundStation/scripts/andy_params.txt
@@ -0,0 +1,27 @@
+lat p -0.0081
+lat i -0.000104
+lat d -0.157
+long p 0.0081
+long i 0.000104
+long d 0.157
+height p 1095.83
+height i 70.08
+height d 4284.1
+pitch p 3.355
+pitch i 0.0
+pitch d 0.0
+roll p 3.355
+roll i 0.0
+roll d 0.0
+yaw p 7.423
+yaw i 0.0
+yaw d 0.0
+pitchv p 3375.4
+pitchv i 0.0
+pitchv d 0.0
+rollv p 3375.4
+rollv i 0.0
+rollv d 0.0
+yawv p 47344.3
+yawv i 0.0
+yawv d 0.0
diff --git a/groundStation/scripts/setpid_batch.sh b/groundStation/scripts/setpid_batch.sh
index 3702acaec..493301906 100755
--- a/groundStation/scripts/setpid_batch.sh
+++ b/groundStation/scripts/setpid_batch.sh
@@ -14,4 +14,4 @@ regex='(lat|long|height|pitch|roll|yaw|pitchv|rollv|yawv) (p|i|d) ([-+]?[0-9]*\.
awk_prog='{print "./Cli setpid" " " "--" $1 " " "-" $2 " " $3}'
# Search, parse, and execute ./Cli commands
-grep -E "$regex" $filename | awk "$awk_prog" | bash
\ No newline at end of file
+grep -E "$regex" $filename | awk "$awk_prog" | bash
diff --git a/groundStation/scripts/tuning.txt b/groundStation/scripts/tuning.txt
new file mode 100644
index 000000000..f4e71dfc2
--- /dev/null
+++ b/groundStation/scripts/tuning.txt
@@ -0,0 +1,6 @@
+lat p -0.01
+lat i 0
+lat d -0.05
+long p 0.01
+long i 0
+long d 0.05
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 109b463af..2e6795663 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -127,10 +127,7 @@ modular_structs_t structs;
// in the future.
static void cb(struct ucart_vrpn_TrackerData * td) {
static int count = 0;
-
- if(!(count % 10)) {
- sendVrpnPacket(td);
- }
+ sendVrpnPacket(td);
count++;
}
@@ -694,7 +691,7 @@ static void client_recv(int fd) {
}
}
- // printf("packet = '");
+ // printf("packetToQuad = '");
// for(int i = 0; i < (int)psize; ++i) {
// printf(" %.2x ", packet[i]);
// }
@@ -704,7 +701,6 @@ static void client_recv(int fd) {
}
-
char * rest = &buffer[newline] + 1;
size_t restLen = (strlen(rest) == 0) ? 1 : strlen(rest);
/* Delete parsed data and move the rest to the left */
@@ -731,7 +727,7 @@ static void quad_recv() {
}
respBufLen += respLen;
- // printf("packet = '");
+ // printf("packetFromQuad = '");
// for(int i = 0; i < (int)respBufLen; ++i) {
// printf(" %.2x ", respBuf[i]);
// }
@@ -801,8 +797,7 @@ static void quad_recv() {
default:
printf("(Backend): message type %d unrecognized\n", m.msg_type);
}
- }
-
+ }
}
static void handleResponseParam(struct metadata *m, uint8_t * data)
@@ -817,7 +812,6 @@ static void handleResponseParam(struct metadata *m, uint8_t * data)
const char * message = cmToString(RESPPARAM_ID, &cm);
size_t len = snprintf(buffer, 128, "%s %f\n", message, cm.value);
- printf("response = %s\n", buffer);
for(int fd = 0; fd <= max_fd; ++fd) {
if (get_client_index(fd) > -1) {
diff --git a/groundStation/src/backend/cmHandler.c b/groundStation/src/backend/cmHandler.c
index 15a34f7ae..a1159c183 100644
--- a/groundStation/src/backend/cmHandler.c
+++ b/groundStation/src/backend/cmHandler.c
@@ -24,14 +24,14 @@ const char * respParamStrings[MAX_PARAM_COMMANDS] = {
"getlatp",
"getlati",
"getlatd",
- "getlat",
"getlongp",
"getlongi",
"getlongd",
- "getlong",
"getheightp",
"getheighti",
"getheightd",
+ "getlat",
+ "getlong",
"getheight"
};
@@ -76,10 +76,9 @@ const char * cmToString(int msgType, const struct controller_message *cm)
} else if (cm->id == X_SETPOINT_ID) {
index = PARAM_LOCAL_Y; // This will change when we make node structure changes
} else if (cm->id == X_SETPOINT_ID) {
- index = PARMA_ALT; // This will change when we make node structure changes
+ index = PARAM_ALT; // This will change when we make node structure changes
} else {
index = (cm->id * MAX_CONTROL_PARAM_ID) + cm->value_id;
- printf("index = %lu\n", index);
}
switch (msgType) {
case DEBUG_ID :
@@ -133,6 +132,7 @@ struct controller_message * stringToCm(const char * string, struct controller_me
break;
}
}
+ // printf("index = %d\n", index);
/* Error, nothing found */
if (index == -1) {
@@ -167,7 +167,7 @@ struct controller_message * stringToCm(const char * string, struct controller_me
cm->id = ALT_ID;
} else if ( i == PARAM_LOCAL_X) {
-
+
cm->id = X_SETPOINT_ID;
cm->value_id = 0;
} else if ( i == PARAM_LOCAL_Y) {
diff --git a/groundStation/src/backend/commands.h b/groundStation/src/backend/commands.h
index 6dda7213d..73ec98f95 100644
--- a/groundStation/src/backend/commands.h
+++ b/groundStation/src/backend/commands.h
@@ -57,11 +57,11 @@ enum ControllerID{
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
- X_SETPOINT_ID, // 09 - X Setpoint
- Y_SETPOINT_ID, // 10 - Y Setpoint
+ LOCAL_X_ID, // 06 - Local X PID // lat
+ LOCAL_Y_ID, // 07 - Local Y PID //long
+ ALT_ID, // 08 - Altitude PID
+ X_SETPOINT_ID, // 09 - X Setpoint //lat
+ Y_SETPOINT_ID, // 10 - Y Setpoint //long
ALT_SETPOINT_ID, // 11 - Z Setpoint
MAX_CONTROLLER_ID // 12 - Just used to keep track of the size
};
diff --git a/groundStation/src/cli/cli_monitor.c b/groundStation/src/cli/cli_monitor.c
index b435b9117..7cc64f54d 100644
--- a/groundStation/src/cli/cli_monitor.c
+++ b/groundStation/src/cli/cli_monitor.c
@@ -143,7 +143,7 @@ int monitor(struct backend_conn * conn) {
printf("PosAtt: H LAT LON P R Y\n");
printf(" :%10.3lf%10.3lf%10.3lf%10.3lf%10.3lf%10.3lf\n",
td.height, td.lateral, td.longitudinal,
- td.pitch, td.roll, td.yaw);
+ td.pitch, td.roll, td.yaw); //TODO: Tracker data long and lat are backwards
monitorcounter++;
pidcounter = ((pidcounter + 1) % PID_NUM_PIDS);
diff --git a/groundStation/src/cli/cli_setpid.c b/groundStation/src/cli/cli_setpid.c
index f9c54c063..b2ca124f5 100644
--- a/groundStation/src/cli/cli_setpid.c
+++ b/groundStation/src/cli/cli_setpid.c
@@ -93,9 +93,9 @@ int cli_setpid(struct backend_conn * conn, int argc, char **argv) {
} else if (setHeight) {
pid_data.controller = PID_HEIGHT;
} else if (setLong) {
- pid_data.controller = PID_LAT;
- } else if (setLat) {
pid_data.controller = PID_LONG;
+ } else if (setLat) {
+ pid_data.controller = PID_LAT;
}
frontend_setpid(conn, &pid_data, mask);
diff --git a/groundStation/src/frontend/pid_common.h b/groundStation/src/frontend/pid_common.h
index 87e61255f..b89e3b1cd 100644
--- a/groundStation/src/frontend/pid_common.h
+++ b/groundStation/src/frontend/pid_common.h
@@ -9,8 +9,8 @@ enum pid_controller {
PID_ROLL_RATE,
PID_YAW_RATE,
PID_HEIGHT, /* Z */
- PID_LONG, /* Y */
PID_LAT, /* X */
+ PID_LONG, /* Y */
PID_NUM_PIDS
};
--
GitLab