Resolve "manual_assist_mode"

groundStation/.gitignore

@@ -43,6 +43,7 @@ BackEnd
 obj
 Cli
 GroundStation
+ManualAssist
 
 #symlinks
 getnodes

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:

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
 

groundStation/src/manual_assist/README.md

+# 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
+
+![alt text](../../documentation/images/usb-controller_edit.jpg "USB 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!**

groundStation/src/manual_assist/joystick.c

+#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);
+}
+

groundStation/src/manual_assist/joystick.h

+#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
+

groundStation/src/manual_assist/manual_assist_main.c

+//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;
+}