From 9ae7336c7a6f8021842a6b3c9124a91a4cded944 Mon Sep 17 00:00:00 2001
From: burneykb <burneykb@iastate.edu>
Date: Mon, 23 Jan 2017 18:47:18 -0600
Subject: [PATCH] Ready to test setsetpoint
---
groundStation/src/cli/cli_setsetpoint.c | 49 +++++++++----------
groundStation/src/cli/cli_setsetpoint.h | 2 +-
groundStation/src/frontend/frontend_setpid.h | 6 +--
.../src/frontend/frontend_setsetpoint.c | 45 +++++++++++++++++
.../src/frontend/frontend_setsetpoint.h | 17 +++++++
groundStation/src/frontend/setpoint_common.h | 11 +++++
6 files changed, 101 insertions(+), 29 deletions(-)
create mode 100644 groundStation/src/frontend/frontend_setsetpoint.c
create mode 100644 groundStation/src/frontend/frontend_setsetpoint.h
create mode 100644 groundStation/src/frontend/setpoint_common.h
diff --git a/groundStation/src/cli/cli_setsetpoint.c b/groundStation/src/cli/cli_setsetpoint.c
index f2ffcb152..7ec7641d5 100644
--- a/groundStation/src/cli/cli_setsetpoint.c
+++ b/groundStation/src/cli/cli_setsetpoint.c
@@ -3,12 +3,14 @@
#include <getopt.h>
#include "cli_setsetpoint.h"
-// #include "frontend_setpid.h"
+#include "frontend_setsetpoint.h"
int cli_set(struct backend_conn * conn, int argc, char **argv) {
int c;
- struct frontend_pid_data pid_data;
+ static int setX = 0, setY = 0, setZ = 0;
+ struct frontend_setpoint_data setpoint_data;
static int needHelp = 0;
+ static int mask;
static struct option long_options[] = {
/* These options don’t set a flag. We distinguish them by their indices. */
{"help", no_argument, &needHelp, 1},
@@ -20,11 +22,28 @@ int cli_set(struct backend_conn * conn, int argc, char **argv) {
/* getopt_long stores the option index here. */
int option_index = 0;
- c = getopt_long(argc, argv, "", long_options, &option_index);
+ c = getopt_long(argc, argv, "x:y:z:", long_options, &option_index);
/* Detect the end of the options. */
if (c == -1)
break;
+
+ switch(c) {
+ case 'x' :
+ setpoint_data.x = atof(optarg);
+ mask |= SET_X;
+ break;
+ case 'y' :
+ setpoint_data.y = atof(optarg);
+ mask |= SET_Y;
+ break;
+ case 'z' :
+ setpoint_data.z = atof(optarg);
+ mask |= SET_Z;
+ break;
+ default :
+ break;
+ }
}
if (needHelp) {
@@ -42,27 +61,7 @@ int cli_set(struct backend_conn * conn, int argc, char **argv) {
printf("Incorrect Usage, run './cli setpid --help' for correct usage.\n");
return 1;
}
-
- if (setRoll) {
- pid_data.controller = PID_ROLL;
- } else if (setYaw) {
- pid_data.controller = PID_YAW;
- } else if (setPitch) {
- pid_data.controller = PID_PITCH;
- } else if (setRollV) {
- pid_data.controller = PID_ROLL_RATE;
- } else if (setPitchV) {
- pid_data.controller = PID_PITCH_RATE;
- } else if (setYawV) {
- pid_data.controller = PID_YAW_RATE;
- } else if (setHeight) {
- pid_data.controller = PID_HEIGHT;
- } else if (setLong) {
- pid_data.controller = PID_LAT;
- } else if (setLat) {
- pid_data.controller = PID_LONG;
- }
-
- frontend_setpid(conn, &pid_data, mask);
+
+ frontend_setsetpoint(conn, &setpoint_data, mask);
return 0;
}
diff --git a/groundStation/src/cli/cli_setsetpoint.h b/groundStation/src/cli/cli_setsetpoint.h
index c379fdd99..7b5dc371f 100644
--- a/groundStation/src/cli/cli_setsetpoint.h
+++ b/groundStation/src/cli/cli_setsetpoint.h
@@ -1,7 +1,7 @@
#ifndef CLI_SETSETPOINT_H
#define CLI_SETSETPOINT_H
-// #include "frontend_setpid.h"
+#include "frontend_setsetpoint.h"
int cli_setsetpoint(struct backend_conn * conn, int argc, char ** argv);
diff --git a/groundStation/src/frontend/frontend_setpid.h b/groundStation/src/frontend/frontend_setpid.h
index f1deb5d3d..5a186ca19 100644
--- a/groundStation/src/frontend/frontend_setpid.h
+++ b/groundStation/src/frontend/frontend_setpid.h
@@ -1,5 +1,5 @@
-#ifndef _FRONTEND_SETPID_H
-#define _FRONTEND_SETPID_H
+#ifndef FRONTEND_SETPID_H
+#define FRONTEND_SETPID_H
#include "pid_common.h"
#include "frontend_common.h"
@@ -14,4 +14,4 @@ int frontend_setpid(
#define SET_D 0x04
#define SET_ALL (SET_P | SET_I | SET_D)
-#endif
+#endif /* FRONTEND_SETPID_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/frontend_setsetpoint.c b/groundStation/src/frontend/frontend_setsetpoint.c
new file mode 100644
index 000000000..f26447921
--- /dev/null
+++ b/groundStation/src/frontend/frontend_setsetpoint.c
@@ -0,0 +1,45 @@
+#include <err.h>
+#include <stdio.h>
+
+#include "frontend_setsetpoint.h"
+#include "setpoint_common.h"
+#include "frontend_common.h"
+
+int frontend_setsetpoint(
+ struct backend_conn * conn,
+ struct frontend_setpoint_data * setpoint_data,
+ int mask)
+{
+ if (conn == NULL) {
+ return -1;
+ }
+
+ char buffer[2048];
+ /* Set the P, I, and D */
+ if (mask & SET_X) {
+ if (snprintf(buffer, 2048,
+ "setsetpointx %f\n",
+ setpoint_data->x) >= 2048) {
+ errx(0, "Buffer to small to format!");
+ }
+ ucart_backendWrite(conn, buffer);
+ }
+ if (mask & SET_Y) {
+ if (snprintf(buffer, 2048,
+ "setsetpointy %f\n",
+ setpoint_data->y) >= 2048) {
+ errx(0, "Buffer to small to format!");
+ }
+ ucart_backendWrite(conn, buffer);
+ }
+ if (mask & SET_Z) {
+ if (snprintf(buffer, 2048,
+ "setsetpointz %f\n",
+ setpoint_data->z) >= 2048) {
+ errx(0, "Buffer to small to format!");
+ }
+ ucart_backendWrite(conn, buffer);
+ }
+
+ return 0;
+}
diff --git a/groundStation/src/frontend/frontend_setsetpoint.h b/groundStation/src/frontend/frontend_setsetpoint.h
new file mode 100644
index 000000000..030417b21
--- /dev/null
+++ b/groundStation/src/frontend/frontend_setsetpoint.h
@@ -0,0 +1,17 @@
+#ifndef FRONTEND_SETSETPOINT_H
+#define FRONTEND_SETSETPOINT_H
+
+#include "setpoint_common.h"
+#include "frontend_common.h"
+
+int frontend_setsetpoint(
+ struct backend_conn * conn,
+ struct frontend_setpoint_data * setpoint_data,
+ int mask);
+
+#define SET_X 0x01
+#define SET_Y 0x02
+#define SET_Z 0x04
+#define SET_ALL (SET_X | SET_Y | SET_Z)
+
+#endif /* FRONTEND_SETSETPOINT_H */
\ No newline at end of file
diff --git a/groundStation/src/frontend/setpoint_common.h b/groundStation/src/frontend/setpoint_common.h
new file mode 100644
index 000000000..25dc8c1ce
--- /dev/null
+++ b/groundStation/src/frontend/setpoint_common.h
@@ -0,0 +1,11 @@
+#ifndef SETPOINT_COMMON_H
+#define SETPOINT_COMMON_H
+
+struct frontend_setpoint_data {
+ float x;
+ float y;
+ float z;
+};
+
+
+#endif /* SETPOINT_COMMON_H */
\ No newline at end of file
--
GitLab