From adbdea1cb57d82569bad84fa991628e3d94919da Mon Sep 17 00:00:00 2001
From: burneykb <burneykb@iastate.edu>
Date: Sat, 18 Feb 2017 21:01:08 -0600
Subject: [PATCH] ready to test
---
groundStation/src/backend/backend.c | 16 ++---
groundStation/src/backend/cmHandler.c | 71 +++++++++----------
groundStation/src/backend/cmHandler.h | 4 +-
groundStation/src/cli/cli_getsetpoint.c | 3 +
.../src/frontend/frontend_getsetpoint.c | 42 +++++++++++
5 files changed, 89 insertions(+), 47 deletions(-)
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 0c3ddf4b1..109b463af 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);
@@ -733,7 +733,7 @@ static void quad_recv() {
// printf("packet = '");
// for(int i = 0; i < (int)respBufLen; ++i) {
- // printf(" 0x%.2x ", respBuf[i]);
+ // printf(" %.2x ", respBuf[i]);
// }
// printf("'\n");
@@ -760,7 +760,6 @@ static void quad_recv() {
respBufLen -=i;
return;
}
-
}
respBufLen = 0;
return;
@@ -777,7 +776,6 @@ static void quad_recv() {
switch (m.msg_type) {
case DEBUG_ID :
- printf();
case PACKETLOG_ID :
case GETPACKETLOGS_ID :
case UPDATE_ID :
@@ -814,12 +812,12 @@ static void handleResponseParam(struct metadata *m, uint8_t * data)
warnx("DecodeResponse error");
return;
}
-
char buffer[128];
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 54a1be157..15a34f7ae 100644
--- a/groundStation/src/backend/cmHandler.c
+++ b/groundStation/src/backend/cmHandler.c
@@ -57,21 +57,30 @@ const char * setParamStrings[MAX_PARAM_COMMANDS] = {
"setlatp",
"setlati",
"setlatd",
- "setlat",
"setlongp",
"setlongi",
"setlongd",
- "setlong",
"setheightp",
"setheighti",
"setheightd",
+ "setlat",
+ "setlong",
"setheight",
};
-
const char * cmToString(int msgType, const struct controller_message *cm)
{
- size_t index = (cm->id * MAX_CONTROL_PARAM_ID) + cm->value_id;
+ size_t index;
+ if (cm->id == X_SETPOINT_ID) {
+ index = PARAM_LOCAL_X; // This will change when we make node structure changes
+ } 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
+ } else {
+ index = (cm->id * MAX_CONTROL_PARAM_ID) + cm->value_id;
+ printf("index = %lu\n", index);
+ }
switch (msgType) {
case DEBUG_ID :
case PACKETLOG_ID :
@@ -148,39 +157,29 @@ struct controller_message * stringToCm(const char * string, struct controller_me
} else if (i <= PARAM_YAW_RATE_D) {
cm->id = YAW_RATE_ID;
- } else if (i <= PARAM_LOCAL_X) {
- // 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) {
- // 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) {
- // 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);
-
- }
+ } else if (i <= PARAM_LOCAL_X_D) {
+
+ cm->id = LOCAL_X_ID;
+ } else if (i <= PARAM_LOCAL_Y_D) {
+
+ cm->id = LOCAL_Y_ID;
+ } else if (i <= PARAM_ALT_D) {
+
+ 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) {
+
+ cm->id = Y_SETPOINT_ID;
+ cm->value_id = 0;
+ } else if ( i == PARAM_ALT) {
+
+ cm->id = ALT_SETPOINT_ID;
+ cm->value_id = 0;
}
}
+ // printf("cm->id = %d\ncm->value_id = %d\n", cm->id, cm->value_id);
return cm;
}
\ No newline at end of file
diff --git a/groundStation/src/backend/cmHandler.h b/groundStation/src/backend/cmHandler.h
index ff749328e..f2761aa9b 100644
--- a/groundStation/src/backend/cmHandler.h
+++ b/groundStation/src/backend/cmHandler.h
@@ -25,14 +25,14 @@ enum paramIndices {
PARAM_LOCAL_X_P ,
PARAM_LOCAL_X_I ,
PARAM_LOCAL_X_D ,
- PARAM_LOCAL_X ,
PARAM_LOCAL_Y_P ,
PARAM_LOCAL_Y_I ,
PARAM_LOCAL_Y_D ,
- PARAM_LOCAL_Y ,
PARAM_ALT_P,
PARAM_ALT_I,
PARAM_ALT_D,
+ PARAM_LOCAL_X ,
+ PARAM_LOCAL_Y ,
PARAM_ALT ,
MAX_PARAM_COMMANDS
};
diff --git a/groundStation/src/cli/cli_getsetpoint.c b/groundStation/src/cli/cli_getsetpoint.c
index be9766139..8cb30bb15 100644
--- a/groundStation/src/cli/cli_getsetpoint.c
+++ b/groundStation/src/cli/cli_getsetpoint.c
@@ -114,6 +114,7 @@ int getSetPointValues(struct backend_conn * conn, struct frontend_setpoint_data
case LONGG :
printf("Longitude: %f\n",
setpoint_data->longg);
+ break;
case PITCH :
printf("Pitch: %f\n",
setpoint_data->pitch);
@@ -121,9 +122,11 @@ int getSetPointValues(struct backend_conn * conn, struct frontend_setpoint_data
case ROLL :
printf("Roll: %f\n",
setpoint_data->roll);
+ break;
case YAW :
printf("Yaw: %f\n",
setpoint_data->yaw);
+ break;
default :
break;
}
diff --git a/groundStation/src/frontend/frontend_getsetpoint.c b/groundStation/src/frontend/frontend_getsetpoint.c
index 1268416f5..b23a5dda4 100644
--- a/groundStation/src/frontend/frontend_getsetpoint.c
+++ b/groundStation/src/frontend/frontend_getsetpoint.c
@@ -1,6 +1,7 @@
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
+#include <err.h>
#include "frontend_getsetpoint.h"
#include "setpoint_common.h"
@@ -48,5 +49,46 @@ int frontend_getsetpoint(
return 1;
}
+ char * response;
+ char tmpresponse[64];
+ char * cmdString;
+ size_t pendingResponses = 1;
+ float value;
+ while (pendingResponses) {
+ response = ucart_backendGetline(conn);
+ if (response == NULL) {
+ warnx("Line not returned from backend");
+ return 1;
+ }
+ strncpy(tmpresponse, response, 64);
+ if (strncmp(tmpresponse, "get", 3) == 0) {
+ cmdString = strtok(tmpresponse, " ");
+ value = strtof(&response[strlen(cmdString)],NULL);
+
+ switch (type) {
+ case HEIGHT :
+ setpoint_data->height = value;
+ break;
+ case LAT :
+ setpoint_data->lat = value;
+ break;
+ case LONGG :
+ setpoint_data->longg = value;
+ break;
+ case PITCH :
+ setpoint_data->pitch = value;
+ break;
+ case ROLL :
+ setpoint_data->roll = value;
+ break;
+ case YAW :
+ setpoint_data->yaw = value;
+ break;
+ }
+
+ pendingResponses--;
+ }
+ }
+
return 0;
}
--
GitLab