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