From e7a2ea0a38d5a2aef45cdb3c6a0826cf628973f7 Mon Sep 17 00:00:00 2001
From: Jake Drahos <j@kedrahos.com>
Date: Mon, 28 Nov 2016 15:27:27 -0600
Subject: [PATCH] Added frontend_setpid
---
groundStation/src/frontend/frontend_setpid.c | 65 ++++++++++++++++++++
groundStation/src/frontend/frontend_setpid.h | 17 +++++
2 files changed, 82 insertions(+)
create mode 100644 groundStation/src/frontend/frontend_setpid.c
create mode 100644 groundStation/src/frontend/frontend_setpid.h
diff --git a/groundStation/src/frontend/frontend_setpid.c b/groundStation/src/frontend/frontend_setpid.c
new file mode 100644
index 000000000..ec3119925
--- /dev/null
+++ b/groundStation/src/frontend/frontend_setpid.c
@@ -0,0 +1,65 @@
+#include <err.h>
+#include <stdio.h>
+
+#include "frontend_setpid.h"
+#include "pid_common.h"
+#include "frontend_common.h"
+
+int frontend_setpid(
+ struct backend_conn * conn,
+ struct frontend_pid_data * pid_data,
+ int mask)
+{
+ if (conn == NULL) {
+ return -1;
+ }
+
+ char * controller;
+ switch (pid_data->controller) {
+ case PID_PITCH:
+ controller = "pitch";
+ break;
+ case PID_ROLL:
+ controller = "roll";
+ break;
+ case PID_YAW:
+ controller = "yaw";
+ break;
+ /* What is the "throttle" pid constant? */
+ default:
+ warnx("Unsupported PID variable");
+ return -1;
+ }
+
+ char buffer[2048];
+ /* Set the P, I, and D */
+ if (mask & SET_P) {
+ if (snprintf(buffer, 2048,
+ "set%sp %f\n",
+ controller,
+ pid_data->p) >= 2048) {
+ errx(0, "Buffer to small to format!");
+ }
+ ucart_backendWrite(conn, buffer);
+ }
+ if (mask & SET_I) {
+ if (snprintf(buffer, 2048,
+ "set%si %f\n",
+ controller,
+ pid_data->i) >= 2048) {
+ errx(0, "Buffer to small to format!");
+ }
+ ucart_backendWrite(conn, buffer);
+ }
+ if (mask & SET_D) {
+ if (snprintf(buffer, 2048,
+ "set%sd %f\n",
+ controller,
+ pid_data->d) >= 2048) {
+ errx(0, "Buffer to small to format!");
+ }
+ ucart_backendWrite(conn, buffer);
+ }
+
+ return 0;
+}
diff --git a/groundStation/src/frontend/frontend_setpid.h b/groundStation/src/frontend/frontend_setpid.h
new file mode 100644
index 000000000..f1deb5d3d
--- /dev/null
+++ b/groundStation/src/frontend/frontend_setpid.h
@@ -0,0 +1,17 @@
+#ifndef _FRONTEND_SETPID_H
+#define _FRONTEND_SETPID_H
+
+#include "pid_common.h"
+#include "frontend_common.h"
+
+int frontend_setpid(
+ struct backend_conn * conn,
+ struct frontend_pid_data * pid_data,
+ int mask);
+
+#define SET_P 0x01
+#define SET_I 0x02
+#define SET_D 0x04
+#define SET_ALL (SET_P | SET_I | SET_D)
+
+#endif
--
GitLab