diff --git a/groundStation/.gitignore b/groundStation/.gitignore
index 626d0161fb7b86803021a9a1bc0e6dd9b6733432..8a84dfc99eef1e265992a59c95b64e44e3b8cbd4 100644
--- a/groundStation/.gitignore
+++ b/groundStation/.gitignore
@@ -43,6 +43,7 @@ BackEnd
obj
Cli
GroundStation
+ManualAssist
#symlinks
getnodes
diff --git a/groundStation/Makefile b/groundStation/Makefile
index b9c4448111551e0ec67f036d52c0a9ce40a7bfba..2ca5036b56e0fed2b3a9cb4de318b766139275d8 100644
--- a/groundStation/Makefile
+++ b/groundStation/Makefile
@@ -6,10 +6,16 @@ GXX=g++
CFLAGS= -Wall -pedantic -Wextra -std=gnu99 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
CXXFLAGS= -Wall -pedantic -Wextra -Wno-reorder -Wno-unused-variable -std=c++0x -g
INCLUDES = $(foreach dir, $(INCDIR), -I$(dir))
-INCDIR= src/vrpn src/vrpn/quat src/vrpn/build $(BESRCDIR) $(CLISRCDIR) $(FESRCDIR) ../quad/inc
+INCDIR= src/vrpn src/vrpn/quat src/vrpn/build $(BESRCDIR) $(CLISRCDIR) $(FESRCDIR) $(MASRCDIR) ../quad/inc
LIBS= -lpthread -lbluetooth -lvrpn -lquat -Lsrc/vrpn/build -Lsrc/vrpn/build/quat -L../quad/lib -lquad_app -lcommands -lgraph_blocks -lcomputation_graph -lm
OBJDIR=obj
+# Manual Assist Specific Variables
+MABINARY=ManualAssist
+MASRCDIR=src/manual_assist
+MASOURCES := $(wildcard $(MASRCDIR)/*.c)
+MAOBJECTS = $(MASOURCES:$(MASRCDIR)/%.c=$(OBJDIR)/%.o)
+
# Backend Specific Variables
BEBINARY=BackEnd
BESRCDIR=src/backend
@@ -34,7 +40,7 @@ FECOBJECTS = $(FECSOURCES:$(FESRCDIR)/%.c=$(OBJDIR)/%.o)
OBJECTS=$(CLIOBJECTS) $(BECOBJECTS) $(BECPPOBJECTS) $(FECOBJECTS)
# Default target
-all: quad logs objdir backend cli $(SYMLINKS) frontend.a GroundStation
+all: quad logs objdir backend cli $(SYMLINKS) frontend.a GroundStation manualassist
quad:
$(MAKE) -C ../quad
@@ -65,6 +71,12 @@ $(BECOBJECTS) : $(OBJDIR)/%.o : $(BESRCDIR)/%.c
$(BECPPOBJECTS) : $(OBJDIR)/%.o : $(BESRCDIR)/%.cpp
$(GXX) $(CXXFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
+manualassist: $(MAOBJECTS) $(FECOBJECTS)
+ $(GCC) $(CFLAGS) $^ -o $(MABINARY) $(INCLUDES) $(LIBS)
+
+$(MAOBJECTS) : $(OBJDIR)/%.o : $(MASRCDIR)/%.c
+ $(GCC) $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
+
vrpn/build:
mkdir -p src/vrpn/build
cd src/vrpn/build && cmake .. && make
@@ -84,6 +96,6 @@ clean_logs:
rm -f logs/*
clean:
- rm -rf $(OBJDIR)/ $(BEBINARY) $(CLIBINARY) $(SYMLINKS)
+ rm -rf $(OBJDIR)/ $(BEBINARY) $(CLIBINARY) $(SYMLINKS) $(MABINARY)
debug:
diff --git a/groundStation/README.md b/groundStation/README.md
index 259c52199225376d2a16ad730ebb2b29a58a9ccb..5444c06b095d7ec9c7e4d2ffbdd493ac02419803 100644
--- a/groundStation/README.md
+++ b/groundStation/README.md
@@ -92,6 +92,7 @@ There are a couple of already written bash scripts in the `scripts/` folder.
- [Getting Started](./documentation/getting_started.md)
- [Packet Information](./documentation/packets.md)
+ - [Manual Assist Mode](./src/manual_assist/README.md)
## Adapters
diff --git a/groundStation/documentation/images/usb-controller.jpg b/groundStation/documentation/images/usb-controller.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..e41c6624bba90ed38e48a20b4cc5dd696700de15
Binary files /dev/null and b/groundStation/documentation/images/usb-controller.jpg differ
diff --git a/groundStation/documentation/images/usb-controller_edit.jpg b/groundStation/documentation/images/usb-controller_edit.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..7c575d527f33f1460b8d58de944e45811c9331b9
Binary files /dev/null and b/groundStation/documentation/images/usb-controller_edit.jpg differ
diff --git a/groundStation/src/manual_assist/README.md b/groundStation/src/manual_assist/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e744dea612b8382f8ec60cb3508583bd5478ec76
--- /dev/null
+++ b/groundStation/src/manual_assist/README.md
@@ -0,0 +1,59 @@
+# Manual Assist Mode
+## About
+Manual Assist Mode is a mode where the quad is set to autonomous mode and then a usb controller is used to control a set point that the quad will move to. This is a seperate application that interfaces with a running backend instance.
+
+## Make Process
+Plug in the usb controller and then check that the device file is there by using the following command
+```bash
+ls /dev/input/
+```
+In the output you should see the a device file that is called **jsX** where X is a number. ( normally **js0** )
+
+**Check the path** at the top of [manual_assist_main.c](./manual_assist_main.c#L13) to **make sure it is mapping to the correct device file** (the jsX file found in the prev step).
+
+Finally run the main [makefile](../../Makefile) for groundstation located in the MicroCART/groundStation/ directory.
+
+## Using
+To use the Manual Assist Mode you will have to initially make the groundStation and ManualAssist executable as described above.
+
+Next **start up a backend instance** the standard way as described by [groundStation README](../../README.md)
+
+Next run through the [standard demo initialization](../../../documentation/how_to_demo.md) steps to **get to the autonomous flight** step.
+
+Once you get the quad past the manual flight mode test, set it to autonomous mode by flipping the **FLAP SWITCH** to **0** on the manual mode normal controller **(The NON USB controller)**
+
+Finally run the ManualAssist program by running the following command from the MicroCART/groundStation/ directory by running the following command
+```bash
+ ./ManualAssist
+```
+
+You can now use the controller to take off by flipping the **channel 7 switch** to the **up** position
+
+Use Joysticks to move your set point within the safety range defined in your [manual_assist_mode.c](manual_assist_main.c)
+
+When finished with your flight flip the **channel 7 switch** to the **down** position to land and flip the switches on the manual mode controller (**NON USB**) to manual and killed
+
+Remember to **end the ManualAssist program** by using the **ctrl** + **c** key combination
+## Controller
+
+
+
+ - **Channel 5 switch**
+ UP = DEBUG ON
+ DOWN = DEBUG OFF
+ - **Channel 7 switch**
+ UP = TAKEOFF script / controlling flight
+ DOWN = LAND script / done flying
+ - **Left Joystick**
+ UP = QUAD height up
+ CENTER = QUAD keep height
+ DOWN = QUAD height down
+ RIGHT / LEFT = nothing
+ - **Right Joystick**
+ UP = QUAD forward
+ DOWN = QUAD backward
+ RIGHT = QUAD right
+ LEFT = QUAD left
+ CENTER = keep X and Y
+
+**NOTE: When flying keep in mind the directions of the camera system!**
diff --git a/groundStation/src/manual_assist/joystick.c b/groundStation/src/manual_assist/joystick.c
new file mode 100644
index 0000000000000000000000000000000000000000..c3c086a8098ee3a012994d68cfa090b227eada0a
--- /dev/null
+++ b/groundStation/src/manual_assist/joystick.c
@@ -0,0 +1,163 @@
+#include "joystick.h"
+
+
+float map_(float in, float in_min, float in_max, float out_min, float out_max)
+{
+ return (in - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+
+float clip_(float in, float min, float max)
+{
+ if (in > max)
+ return max;
+ if (in < min)
+ return min;
+ return in;
+}
+
+
+struct joystick* joystick_init(const char* path, int blocking)
+{
+ struct joystick* js = malloc(sizeof(struct joystick));
+ if (!js)
+ {
+ return js;
+ }
+ int options = O_RDONLY | O_NONBLOCK;
+ if (blocking)
+ options = O_RDONLY;
+ js->fd = open(path, options);
+ if (js->fd == -1)
+ {
+ char err[64];
+ sprintf(err, "Failed to initialize joystick \"%s\"", path);
+ perror(err);
+ free(js);
+ js = NULL;
+ }
+ return js;
+}
+
+
+int joystick_read(struct joystick* js)
+{
+ int status = read(js->fd, &(js->e), sizeof(js->e));
+ if (status == -1 && errno != EAGAIN)
+ {
+ perror("Failed to read joystick");
+ free(js);
+ return 0;
+ }
+
+ int val = js->e.value;
+
+ if (js->e.type == JS_EVENT_AXIS || (js->e.type == (JS_EVENT_AXIS | JS_EVENT_INIT)))
+ {
+ float norm = 0.0;
+
+ if (js->e.number == JS_THROTTLE_AXIS)
+ {
+ if (val < JS_THROTTLE_MID)
+ norm = map_(val, JS_THROTTLE_MIN, JS_THROTTLE_MID, 0.0f, 0.5f);
+ else
+ norm = map_(val, JS_THROTTLE_MID, JS_THROTTLE_MAX, 0.5f, 1.0f);
+ js->axis[0] = clip_(norm, 0.0f, 1.0f);
+ }
+ else if (js->e.number == JS_YAW_AXIS)
+ {
+ if (val < JS_YAW_MID)
+ norm = map_(val, JS_YAW_MIN, JS_YAW_MID, 0.0f, 0.5f);
+ else
+ norm = map_(val, JS_YAW_MID, JS_YAW_MAX, 0.5f, 1.0f);
+ js->axis[1] = clip_(norm, 0.0f, 1.0f);
+ }
+ else if (js->e.number == JS_PITCH_AXIS)
+ {
+ if (val < JS_PITCH_MID)
+ norm = map_(val, JS_PITCH_MIN, JS_PITCH_MID, 0.0f, 0.5f);
+ else
+ norm = map_(val, JS_PITCH_MID, JS_PITCH_MAX, 0.5f, 1.0f);
+ js->axis[2] = clip_(norm, 0.0f, 1.0f);
+ }
+ else if (js->e.number == JS_ROLL_AXIS)
+ {
+ if (val < JS_ROLL_MID)
+ norm = map_(val, JS_ROLL_MIN, JS_ROLL_MID, 0.0f, 0.5f);
+ else
+ norm = map_(val, JS_ROLL_MID, JS_ROLL_MAX, 0.5f, 1.0f);
+ js->axis[3] = clip_(norm, 0.0f, 1.0f);
+ }
+ }
+ else if (js->e.type == JS_EVENT_BUTTON || (js->e.type == (JS_EVENT_BUTTON | JS_EVENT_INIT)))
+ {
+ if (js->e.number == JS_LEFT_SWITCH)
+ js->button[0] = val;
+ else if (js->e.number == JS_RIGHT_SWITCH)
+ js->button[1] = val;
+ }
+ return 1;
+}
+
+
+float joystick_get_throttle(struct joystick* js)
+{
+ return js->axis[0];
+}
+
+
+float joystick_get_yaw(struct joystick* js)
+{
+ return js->axis[1];
+}
+
+
+float joystick_get_pitch(struct joystick* js)
+{
+ return js->axis[2];
+}
+
+
+float joystick_get_roll(struct joystick* js)
+{
+ return js->axis[3];
+}
+
+
+float joystick_get_left_switch(struct joystick* js)
+{
+ return js->button[0];
+}
+
+
+float joystick_get_right_switch(struct joystick* js)
+{
+ return js->button[1];
+}
+
+
+void joystick_info(struct joystick* js)
+{
+ int version, axes, buttons;
+ version = axes = buttons = 0;
+ char name[128];
+ ioctl(js->fd, JSIOCGAXES, &axes);
+ ioctl(js->fd, JSIOCGBUTTONS, &buttons);
+ ioctl(js->fd, JSIOCGVERSION, &version);
+ ioctl(js->fd, JSIOCGNAME(sizeof(name)), &name);
+ printf("name: %s\n"
+ "version: %d\n"
+ "axes: %d\n"
+ "buttons: %d\n",
+ name,
+ version,
+ axes,
+ buttons);
+}
+
+
+void joystick_destroy(struct joystick* js)
+{
+ free(js);
+}
+
diff --git a/groundStation/src/manual_assist/joystick.h b/groundStation/src/manual_assist/joystick.h
new file mode 100644
index 0000000000000000000000000000000000000000..44a7b9a945ea84e3d0f58ec179b640ae2205d5f7
--- /dev/null
+++ b/groundStation/src/manual_assist/joystick.h
@@ -0,0 +1,106 @@
+#ifndef JOYSTICK_H
+#define JOYSTICK_H
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <linux/joystick.h>
+
+extern int errno;
+
+/* example of usage, for help see https://www.kernel.org/doc/Documentation/input/joystick-api.txt
+
+#include <stdio.h>
+#include "joystick.h"
+
+
+int main(void) {
+ struct joystick* js = joystick_init("/dev/input/js0", JS_NONBLOCKING);
+ if (!js)
+ {
+ printf("Aborting because joystick did not initialize correctly...\n");
+ return 1;
+ }
+
+ joystick_info(js);
+
+ while(1)
+ {
+ if (!joystick_read(js))
+ {
+ printf("Aborting because joystick could not be read...\n");
+ return 1;
+ }
+ printf("%1.2f\t"
+ "%1.2f\t"
+ "%1.2f\t"
+ "%1.2f\t"
+ "%1.2f\t"
+ "%1.2f\n",
+ joystick_get_throttle(js),
+ joystick_get_yaw(js),
+ joystick_get_pitch(js),
+ joystick_get_roll(js),
+ joystick_get_left_switch(js),
+ joystick_get_right_switch(js));
+ }
+
+ joystick_destroy(js);
+ return 0;
+}
+*/
+
+
+#define JS_NONBLOCKING 0
+#define JS_BLOCKING 1
+
+/* setup and calibration for GREAT PLANES InterLink Elite */
+#define JS_LEFT_SWITCH 0
+#define JS_RIGHT_SWITCH 1
+#define JS_THROTTLE_AXIS 2
+#define JS_THROTTLE_MIN 21620
+#define JS_THROTTLE_MID 0
+#define JS_THROTTLE_MAX -22296
+#define JS_YAW_AXIS 4
+#define JS_YAW_MIN -20607
+#define JS_YAW_MID 0
+#define JS_YAW_MAX 25336
+#define JS_PITCH_AXIS 1
+#define JS_PITCH_MIN 21957
+#define JS_PITCH_MID 0
+#define JS_PITCH_MAX -19594
+#define JS_ROLL_AXIS 0
+#define JS_ROLL_MIN -20945
+#define JS_ROLL_MID 0
+#define JS_ROLL_MAX 25336
+
+
+struct joystick
+{
+ int fd;
+ struct js_event e;
+ float axis[4];
+ float button[2];
+};
+
+
+float map_(float in, float in_min, float in_max, float out_min, float out_max);
+float clip_(float in, float min, float max);
+
+
+struct joystick* joystick_init(const char* path, int blocking);
+int joystick_read(struct joystick* js);
+float joystick_get_throttle(struct joystick* js);
+float joystick_get_yaw(struct joystick* js);
+float joystick_get_pitch(struct joystick* js);
+float joystick_get_roll(struct joystick* js);
+float joystick_get_left_switch(struct joystick* js);
+float joystick_get_right_switch(struct joystick* js);
+void joystick_info(struct joystick* js);
+void joystick_destroy(struct joystick* js);
+
+
+#endif
+
diff --git a/groundStation/src/manual_assist/manual_assist_main.c b/groundStation/src/manual_assist/manual_assist_main.c
new file mode 100644
index 0000000000000000000000000000000000000000..6a5f1fe191722fdc2bf47fd882865a21cf1011c7
--- /dev/null
+++ b/groundStation/src/manual_assist/manual_assist_main.c
@@ -0,0 +1,312 @@
+//system include
+#include <stdlib.h>
+#include <stdio.h>
+#include <signal.h>
+
+//user includes
+#include "joystick.h"
+#include "frontend_common.h"
+#include "frontend_tracker.h"
+#include "frontend_param.h"
+
+//Path to the joystick device
+#define JOYSTICK_PATH "/dev/input/js0"
+
+//Variables used to calculate the allowed setpoint change per iteration of the main loop
+#define WAIT_TIME_USEC 5000
+#define METER_P_SEC 0.66f
+#define SETPOINT_THRESHOLD (((float) WAIT_TIME_USEC) / 1000000) * METER_P_SEC
+
+//Variables used to update setpoints through the frontend
+#define QUAD_X 9
+#define QUAD_Y 10
+#define QUAD_Alt 11
+#define QUAD_Yaw 12
+#define PARAM 0
+
+//Variables that define the bounds that the user can fly between
+#define MAX_Y 1.2f
+#define MIN_Y -1.2f
+#define MAX_X 1.5f
+#define MIN_X -1.5f
+#define MAX_Alt -0.7f
+#define MIN_Alt -1.5f
+#define MAX_Yaw 160f
+#define MIN_Yaw -160f
+
+
+#define DELTA 0.05f
+#define MIDPOINT 0.5f
+
+//Checks whether a value from the joystick is recognized as a midpoint and hence the quad will not move
+#define isMidPoint(value) (value < (MIDPOINT + DELTA) && value > (MIDPOINT - DELTA)) ? 1 : 0
+
+int exit_flag = 0;
+
+//Connection to backend
+struct backend_conn *conn = NULL;
+/**
+ * Handler function for exit signals. This allows the backend to properly close.
+ */
+static void sig_exit_handler(int s) {
+ printf("Exiting with condition %d\n\r", s);
+ exit_flag = 1;
+}
+
+//Adjusts the setpoints so that there is not a jump once the value is not recognized
+float adjustJoystickValue(float value) {
+ if (value > 0.5f) {
+ return value - DELTA;
+ }
+ return value + DELTA;
+}
+
+int main(int argc, char *argv[]) {
+ //Pointer to joystick object
+ struct joystick *js;
+
+ //Struct containing VRPN information
+ struct frontend_tracker_data td;
+
+ //Struct containing setpoint update information for frontend
+ struct frontend_param_data pd;
+
+ //Whether or not the quad has taken off
+ int takeoff = 0;
+
+ //Temporary status variable used to hold return values
+ int status = 0;
+
+ //Whether or not we have gotten the current position of the quad upon start of user control
+ int initialize_setpoint = 0;
+
+ //Joystick channel values
+ float js_throttle, js_yaw, js_pitch, js_roll, js_left_switch, js_right_switch;
+
+ //Quad setpoint related variables (Note - quad_Alt is Z)
+ float quad_X, quad_Y, quad_Alt, quad_yaw;
+
+ float temp_setpoint = 0;
+ //Quad setpoint changed variables
+ int quad_X_modified, quad_Y_modified, quad_Alt_modified, quad_yaw_modified;
+ quad_X_modified = quad_Y_modified = quad_Alt_modified = quad_yaw_modified = 0;
+
+ //Setup exit signals
+ signal(SIGINT, sig_exit_handler);
+ signal(SIGABRT, sig_exit_handler);
+ signal(SIGQUIT, sig_exit_handler);
+ signal(SIGTERM, sig_exit_handler);
+
+ //Initialize joystick
+ js = joystick_init(JOYSTICK_PATH, JS_NONBLOCKING);
+ if (!js) {
+ printf("Aborting because joystick did not initialize correctly...\n");
+ return 1;
+ }
+
+ //Prints out info about the device connected
+ joystick_info(js);
+
+ //Create a connection to the backend
+ conn = ucart_backendConnect();
+ if (conn == NULL) {
+ printf("Failed to connect to backend, exitting...\r\n");
+ joystick_destroy(js);
+ return 1;
+ }
+ printf("Manual Assist Mode Starting...\r\n");
+
+ printf("Setpoint Threshold = %f\r\n", SETPOINT_THRESHOLD);
+
+ while (!exit_flag) {
+ //Read to update values
+ if (joystick_read(js) == 1) {
+ //Get values from joystick
+ js_throttle = joystick_get_throttle(js);
+ //NOTE - yaw control is not yet supported
+ js_yaw = joystick_get_yaw(js);
+ js_pitch = joystick_get_pitch(js);
+ js_roll = joystick_get_roll(js);
+ js_left_switch = joystick_get_left_switch(js);
+ js_right_switch = joystick_get_right_switch(js);
+
+ //Print debug info using the left switch on the controller
+ if (js_left_switch > 0.5f) {
+ printf("Throttle: %f\r\n", js_throttle);
+ printf("Yaw: %f\r\n", js_yaw);
+ printf("Pitch: %f\r\n", js_pitch);
+ printf("Roll: %f\r\n", js_roll);
+ printf("Left Switch: %f\r\n", js_left_switch);
+ printf("Right Switch: %f\r\n", js_right_switch);
+ }
+
+ //Right switch controls takeoff and touch down
+ if (!takeoff && js_right_switch > .5) {
+ //TODO - may not want to use system calls...
+ //call takeoff script
+ printf("TAKING OFF...\r\n");
+ status = system("./scripts/take_off.sh");
+ if (status == -1) {
+ printf("System call failed...\r\n");
+ exit_flag = 1;
+ }
+ takeoff = 1;
+ usleep(5000000);
+ printf("You now have control...\r\n");
+ } else if (takeoff && js_right_switch < .5) {
+ //TODO - may not want to use system calls...
+ //call touchdown script
+ printf("LANDING QUAD...\r\n");
+ status = system("./scripts/touch_down.sh");
+ if (status == -1) {
+ printf("System call failed...\r\n");
+ exit_flag = 1;
+ }
+ takeoff = 0;
+ usleep(5000000);
+ initialize_setpoint = 0;
+ quad_X_modified = quad_Y_modified = quad_Alt_modified = quad_yaw_modified = 0;
+ printf("You have control, but you must takeoff first...\r\n");
+ }
+
+ if (takeoff) {
+ if (!initialize_setpoint) {
+ //grab setpoint
+ initialize_setpoint = 1;
+ status = frontend_track(conn, &td);
+ if (status) {
+ exit_flag = 1;
+ } else {
+ quad_X = td.longitudinal;
+ quad_Y = td.lateral;
+ quad_Alt = td.height;
+ quad_yaw = td.yaw;
+ }
+ } else {
+ //check all joystick values to make sure that they are not in the middle
+ if (!isMidPoint(js_throttle)) {
+ js_throttle = adjustJoystickValue(js_throttle);
+ quad_Alt_modified = 1;
+ temp_setpoint = -(js_throttle - 0.5f);
+ quad_Alt += temp_setpoint * SETPOINT_THRESHOLD;
+ if (quad_Alt < MIN_Alt) {
+ quad_Alt = MIN_Alt;
+ } else if (quad_Alt > MAX_Alt) {
+ quad_Alt = MAX_Alt;
+ }
+ }
+ if (!isMidPoint(js_pitch)) {
+ js_pitch = adjustJoystickValue(js_pitch);
+ quad_X_modified = 1;
+ temp_setpoint = (js_pitch - 0.5f);
+ quad_X += temp_setpoint * SETPOINT_THRESHOLD;
+ if (quad_X < MIN_X) {
+ quad_X = MIN_X;
+ } else if (quad_X > MAX_X) {
+ quad_X = MAX_X;
+ }
+ }
+ if (!isMidPoint(js_roll)) {
+ js_roll = adjustJoystickValue(js_roll);
+ quad_Y_modified = 1;
+ temp_setpoint = (js_roll - 0.5f);
+ quad_Y += temp_setpoint * SETPOINT_THRESHOLD;
+ if (quad_Y < MIN_Y) {
+ quad_Y = MIN_Y;
+ } else if (quad_Y > MAX_Y) {
+ quad_Y = MAX_Y;
+ }
+ }
+ //if (!isMidPoint(js_yaw)) {
+ //TODO - not yet supported
+ //js_yaw = adjustJoystickValue(js_yaw);
+ //quad_yaw_modified = 1;
+ //temp_setpoint = (js_yaw - 0.5f);
+ //quad_yaw += temp_setpoint * SETPOINT_THRESHOLD;
+ //if (quad_yaw < MIN_Yaw) {
+ // quad_yaw = MIN_Yaw;
+ //} else if (quad_yaw > MAX_Yaw) {
+ // quad_yaw = MAX_Yaw;
+ //}
+ //}
+
+ //check if any of the setpoints have been changed,
+ //if so send a message to backend
+ if (quad_Alt_modified) {
+ pd.block = QUAD_Alt;
+ pd.param = PARAM;
+ pd.value = quad_Alt;
+ status = frontend_setparam(conn, &pd);
+ if (status) {
+ printf("frontend_setparam failed...\r\n");
+ exit_flag = 1;
+ }
+ }
+ if (quad_X_modified) {
+ pd.block = QUAD_X;
+ pd.param = PARAM;
+ pd.value = quad_X;
+ status = frontend_setparam(conn, &pd);
+ if (status) {
+ printf("frontend_setparam failed...\r\n");
+ exit_flag = 1;
+ }
+
+ }
+ if (quad_Y_modified) {
+ pd.block = QUAD_Y;
+ pd.param = PARAM;
+ pd.value = quad_Y;
+ status = frontend_setparam(conn, &pd);
+ if (status) {
+ printf("frontend_setparam failed...\r\n");
+ exit_flag = 1;
+ }
+
+ }
+ //if (quad_yaw_modified) {
+ //TODO - not yet supported
+ //pd.block = QUAD_Yaw;
+ //pd.param = PARAM;
+ //pd.value = quad_yaw;
+ //status = frontend_setparam(conn, &pd);
+ //if (status) {
+ // printf("frontend_setparam failed...\r\n");
+ // exit_flag = 1;
+ //}
+ //}
+ quad_X_modified = quad_Y_modified = quad_Alt_modified = quad_yaw_modified = 0;
+ }
+ }
+ } else {
+ printf("Aborting because joystick could not be read...\r\n");
+ exit_flag = 1;
+ }
+ usleep(5000);
+ }
+
+ printf("Manual Assist Mode Exiting...\r\n");
+
+ //If quad is still in the air when exitting, make sure to land it
+ if (takeoff) {
+ printf("LANDING QUAD...\r\n");
+ status = system("./scripts/touch_down.sh");
+ if (status == -1) {
+ printf("System call failed...\r\n");
+ exit_flag = 1;
+ }
+ takeoff = 0;
+ usleep(5000000);
+ }
+
+ //Cleanup joystick
+ joystick_destroy(js);
+
+ //Cleanup backend connection
+ if (conn) {
+ ucart_backendDisconnect(conn);
+ conn = NULL;
+ }
+
+ return 0;
+}