Skip to content
Snippets Groups Projects
frontend_getpid.c 1.89 KiB
Newer Older
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <err.h>
burneykb's avatar
burneykb committed
#include "frontend_getpid.h"
#include "pid_common.h"
burneykb's avatar
burneykb committed

/* Get a specified PID.
 *
 * Example:
 *
 * struct frontend_pid_data pid_data;
 * pid_data.pid = PID_PITCH;
burneykb's avatar
burneykb committed
 * if (frontend_getpid(conn, &pid_data)) {
 * 		error
 * } else {
 * 		pid_data.p, pid_data.i, and pid_data.d are filled
 * }
 *
 * Returns 0 on success, 1 on error
 */
int frontend_getpid(
		struct backend_conn * conn, struct frontend_pid_data * pid_data) {
	char line[100] = "";
	switch (pid_data->controller) {
		case PID_PITCH :
			strncpy(line, "getpitchp\ngetpitchi\ngetpitchd\n", 30);
		case PID_ROLL :
			strncpy(line, "getrollp\ngetrolli\ngetrolld\n", 27);
		case PID_YAW :
			strncpy(line, "getyawp\ngetyawi\ngetyawd\n", 24);
			break;
		case PID_PITCH_RATE :
			strncpy(line, "getpitchratep\ngetpitchratei\ngetpitchrated\n", 33);
			break;
		case PID_ROLL_RATE :
			strncpy(line, "getrollratep\ngetrollratei\ngetrollrated\n", 30);
			break;
		case PID_YAW_RATE :
			strncpy(line, "getyawratep\ngetyawratei\ngetyawrated\n", 27);
			break;
		case PID_HEIGHT :
			strncpy(line, "getheightp\ngetheighti\ngetheightd\n", 33);
			break;
		case PID_LAT :
			strncpy(line, "getlatp\ngetlati\ngetlatd\n", 24);
			break;
		case PID_LONG :
			strncpy(line, "getlongp\ngetlongi\ngetlongd\n", 27);
			break;
		default :
			return 1;
	}

	int size;
	if((size = ucart_backendWrite(conn, line)) < 0 ) {
		return 1;
	}

	char * response;
	for (;;) {
		response = ucart_backendGetline(conn);
		if (response == NULL) {
			warnx("Line not returned from backend");
			return 1;
		}
		printf("received : %s\n", response);
		// if (strncmp(response, MAGIC, strlen(MAGIC)) == 0) {
		// 	break;
		// }
	}

	// if (strncmp(response, MAGIC " ERROR", strlen(MAGIC " ERROR")) == 0) {
	// 	warnx("Backend returned an error: %s", strstr(response, "ERROR"));
	// 	return 1;
	// }

burneykb's avatar
burneykb committed
	return 0;