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