コード例 #1
0
ファイル: picopter.cpp プロジェクト: picopter/picopter
/*
 *	terminate
 *		If the program receives a SIGTERM or SIGINT (Control+C), stop the copter and exit
 *		gracefully.
 */
void terminate(int signum) {
	cout << "\033[33m[THRIFT]\033[0m Signal " << signum << " received. Stopping copter. Exiting." << endl;
	handlerInternal->allStop();
	exitProgram = true;
	
	usleep(500000);
	
	fb.setFB_Data(&stop);
	printFB_Data(&stop);
	
	usleep(500000);
	
	cam.close();
	fb.close();
	gps.close();
	imu.close();
	thriftServer->stop();
}
コード例 #2
0
ファイル: picopter.cpp プロジェクト: picopter/picopter
bool initialise(FlightBoard *fb, GPS *gps, IMU *imu, CAMERA_STREAM *cam) {
	cout << "\033[36m[COPTER]\033[0m Initialising." << endl;
	
	/* Initialise WiringPi */
	gpio::startWiringPi();
	
	/* Initialise Flight Board */
	if(fb->setup() != FB_OK) {
		cout << "\033[1;31m[COPTER]\033[0m Error setting up flight board.  Terminating program" << endl;
		return false;
	}
	fb->start();
	
	/* Initialise GPS */
	if(gps->setup() != GPS_OK) {
		cout << "\033[1;31m[COPTER]\033[0m Error setting up GPS. Will retry continuously." << endl;
		while (gps->setup() != GPS_OK) usleep(1000000);
	}
	gps->start();
	cout << "\033[36m[COPTER]\033[0m GPS detected." << endl;
	
	/* Initialise IMU 
	if(imu->setup() != IMU_OK) {
		cout << "\033[1;31m[COPTER]\033[0m Error setting up IMU. Will retry continuously." << endl;
		while (imu->setup() != IMU_OK) usleep(1000);
	}
	imu->start();
	cout << "\033[36m[COPTER]\033[0m IMU detected." << endl;*/
	
	
	/* Initialise Camera */
	if (cam->setup() != CAMERA_OK) {
		cout << "\033[1;31m[COPTER]\033[0m Error setting up camera. Will retry continuously." << endl;
		while (cam->setup() != CAMERA_OK) usleep(1000000);
	}
	cam->setMode(3);
	cam->start();

	return true;
}
コード例 #3
0
ファイル: run_waypoints3.cpp プロジェクト: picopter/picopter
int main(int argc, char* argv[]) {
	
	//Setup exit signal
	struct sigaction signalHandler;	
	signalHandler.sa_handler = terminate;
	sigemptyset(&signalHandler.sa_mask);
	signalHandler.sa_flags = 0;
	
	sigaction(SIGTERM, &signalHandler, NULL);
	sigaction(SIGINT,  &signalHandler, NULL);

	//Setup hardware
	FlightBoard fb;
	GPS gps;
	IMU imu;
	CAMERA_STREAM cam;
	
	hardware hardware_list = initialise(&fb, &gps, &imu, &cam);
	//Turn off camera
	cam.close();
	hardware_list.CAM_Working = false;
	
	//Start loging
	Logger log = Logger("bax_waypoints3.csv");
	
	//Get waypoints
	deque<coord> waypoints_list = deque<coord>();
	populate_waypoints_list(&waypoints_list);
	
	
	//Start loop
	waypoints_loop3(hardware_list, log, waypoints_list, CONFIG_FILE);
	
	//Close hardware
	fb.stop();
	gps.close();
	imu.close();
	log.closeLog();
}
コード例 #4
0
ファイル: face_red_object.cpp プロジェクト: kishimi8/picopter
int main(int argc, char* argv[]) {
	cout << "Starting program" << endl;
	
	//Signal to exit program.
	struct sigaction signalHandler;	
	signalHandler.sa_handler = terminate;
	sigemptyset(&signalHandler.sa_mask);
	signalHandler.sa_flags = 0;
	
	sigaction(SIGTERM, &signalHandler, NULL);
	sigaction(SIGINT,  &signalHandler, NULL);
	
	//Setup parameters
	ConfigParser::ParamMap parameters;
	parameters.insert("TOL_rotate", &TOL_rotate);
	parameters.insert("KP_rotate", &KP_rotate);
	parameters.insert("SPIN_SPEED", &SPIN_SPEED);
	parameters.insert("MAIN_LOOP_SLEEP", &MAIN_LOOP_SLEEP);
	ConfigParser::loadParameters(TABLE_NAME, &parameters, PARAMETER_FILE);
	
	
	//Startup sensors
	gpio::startWiringPi();
	
	FlightBoard fb = FlightBoard();
	if(fb.setup() != FB_OK) {
		cout << "Error setting up flight board." << endl;
		return -1;
	}
	fb.start();
	FB_Data course = {0, 0, 0, 0};
	FB_Data stop = {0, 0, 0, 0};
	
	CAMERA_VAR3 cam = CAMERA_VAR3();
	if(cam.setup(PARAMETER_FILE) != CAMERA_OK) {
		cout << "Error setting up camera." << endl;
        fb.stop();
		return -1;
	}
	cam.start();
	ObjectLocation red_object;

	int state = 0;			//State machine implemetation
                            //State 0;	Manual mode
                            //State 1;	Object detected, face object + gimbal
                            //State 2;	No object detected -> wait there
	
	usleep(700);	//wait for laggy camera stream			
	
	
	
	//Setup curses
	initscr();
	start_color();
	init_pair(1, COLOR_GREEN, COLOR_BLACK);
	init_pair(2, COLOR_CYAN, COLOR_BLACK);
	refresh();
	int LINES, COLUMNS;
	getmaxyx(stdscr, LINES, COLUMNS);
	
	//Set up title window
	int TITLE_HEIGHT = 4;
	WINDOW *title_window = newwin(TITLE_HEIGHT, COLUMNS -1, 0, 0);
	wattron(title_window, COLOR_PAIR(1));
	wborder(title_window, ' ' , ' ' , '-', '-' , '-', '-', '-', '-');
	wmove(title_window, 1, 0);
	wprintw(title_window, "\t%s\t\n", "FACE_RED_OBJECT");
	wprintw(title_window, "\t%s\t\n", "Hexacopter will rotate on spot to look at red object.");
	wrefresh(title_window);
	
	//Set up messages window
	WINDOW *msg_window = newwin(LINES - TITLE_HEIGHT -1, COLUMNS -1, TITLE_HEIGHT, 0);
	wattron(msg_window, COLOR_PAIR(2));
	
	usleep(1000);				
	
	while(!exitProgram) {
	
		
		
		if(!gpio::isAutoMode()) {					//If not in autonomous mode
			state = 0;

		} else if(cam.objectDetected()) {			//If object detected
            state = 1;
		} else {
			state = 2;								//No object found, wait for it.
		}
		
		/*
		cout << "State:\t" << state << endl;
		cout << "Framerate:\t" << cam.getFramerate() << endl;
		*/
		wclear(msg_window);
		wprintw(msg_window, "\n");
		wprintw(msg_window, "State:\t%d\n", state);
		wprintw(msg_window, "Framerate:\t%1.4f\n", cam.getFramerate());
		wprintw(msg_window, "\n");

		switch(state) {
			case 0:											//Case 0:	Not in auto mode, standby
				fb.setFB_Data(&stop);							//Stop moving
				/*
				printFB_Data(&stop);
				cout << "In manual mode, standby" << endl;		//PRINT SOMETHING
				*/
				wprintw(msg_window, "In manual mode, standby\n");
				wprintw(msg_window, "%s", (stringFB_Data(&stop)).c_str());
				break;
			
			case 1:											//Case 1:	Object detected, face and point gimbal at it.
				cam.getObjectLocation(&red_object);				//GET OBJECT LOCATION
				setCourse_faceObject(&course, &red_object);		//P ON OBJECT
                setCourse_moveGimbal(&course, &red_object);
				fb.setFB_Data(&course);
				/*
				printFB_Data(&course);
				*/
				wprintw(msg_window, "Red object detected\n");
				wprintw(msg_window, "%s", (stringFB_Data(&course)).c_str());
				break;
			

			case 2:											//Case 2 wait there.
			default:
                setCourse_stopTurning(&course);
				fb.setFB_Data(&course);
				/*
				printFB_Data(&course);							//STOP
				cout << "No object." << endl;					//PRINT: NO OBJECT FOUND.
				*/
				wprintw(msg_window, "No red objects detected\n");
				wprintw(msg_window, "%s", (stringFB_Data(&course)).c_str());
				break;
		}
		
		wrefresh(msg_window);
		boost::this_thread::sleep(boost::posix_time::milliseconds(MAIN_LOOP_SLEEP));
	
	}
	endwin();
    fb.stop();
	cam.stop();
	cam.close();
	return 0;
}
コード例 #5
0
ファイル: waypoints.cpp プロジェクト: picopter/picopter
/*
 *	flightLoop
 *		1) Initialise copter systems
 *		2) Wait for user allowance to fly.
 *		3) Read in list of waypoints
 *		4) Fly to first waypoint, wait, fly to next, etc..
 *		5) If the end is reached, stop.
 *		
 *		The user may stop the flight at any time. It will continue if the user resumes. At any
 *		point, the user may stop the current flight path and change the waypoint configuration. Upon
 *		resuming flight, the copter will read in the new list of waypoints and start at the
 *		beginning.
 */
void waypointsFlightLoop(FlightBoard &fb, GPS &gps, IMU &imu, Buzzer &buzzer, Logger &logs, deque<coord> &waypoints_list) {	
	cout << "\033[1;32m[WAYPTS]\033[0m Waypoints thread initiated, travelling to the following waypoints:" << endl;
	
	char str_buf[BUFSIZ];
	bool useimu = false;	// Manual setting for debug
	double yaw = 0;	
	GPS_Data gps_data;
	
	for(size_t i = 0; i != waypoints_list.size(); i++) {
		cout << '         ' << i+1 << ": (" << waypoints_list[i].lat << ", " << waypoints_list[i].lon << ")" << endl;
	}
	
	state = 6;
	while ( !gpio::isAutoMode() ) usleep(1000000);
	
	/*if (!useimu) {
		buzzer.playBuzzer(0.25, 100, 100);
		state = 5;
		yaw = inferBearing(&fb, &gps, &logs);
	}*/
	
	coord		currentCoord = {-1, -1};
	double		distanceToNextWaypoint;
	double		bearingToNextWaypoint;
	FB_Data		course = {0, 0, 0, 0};
	int			pastState = -1;

	try {	// Check for any errors, and stop the copter.
		while(!exitProgram) {
			gps.getGPS_Data(&gps_data);
			currentCoord.lat = gps_data.latitude;
			currentCoord.lon = gps_data.longitude;
			
			if (wp_it == waypoints_list.size()) {
				wp_it = 0;
				userState = 0;
			}

			distanceToNextWaypoint = calculate_distance(currentCoord, waypoints_list[wp_it]);
			bearingToNextWaypoint = calculate_bearing(currentCoord, waypoints_list[wp_it]);
			//if (useimu) yaw = getYaw(&imu);
			yaw = gps_data.heading;			

			/* State 4: Manual mode. */
			if (!gpio::isAutoMode()) {
				state = 4;
				wp_it = 0;
				exitProgram = true;
			/* State 0: All stop */
			} else if (waypoints_list.empty() || userState == 0 ) {
				state = 11;
				wp_it = 0;
				exitProgram = true;
			/* State 3: Error. */
			} else if (state == 3 || !checkInPerth(&currentCoord)) {
				state = 3;
				exitProgram = true;
			/* State 2: At waypoint. */
			} else if (distanceToNextWaypoint < WAYPOINT_RADIUS) {
				state = 2;
			/* State 1: Travelling to waypoint. */
			} else {
				state = 1;
			}
			
			/* Only give output if the state changes. Less spamey.. */
			if (pastState != state) {
				switch (state) {
					case 0:
						cout << "\033[1;32m[WAYPTS]\033[0m All stop." << endl;
						break;
					case 1:
						cout << "\033[1;32m[WAYPTS]\033[0m Travelling to waypoint " << wp_it << ", at (" << waypoints_list[wp_it].lat << "," << waypoints_list[wp_it].lon << ")" << endl;
						break;
					case 2:
						cout << "\033[1;32m[WAYPTS]\033[0m At waypoint" << wp_it << "." << endl;
						break;
					case 3:
						cout << "\033[31m[WAYPTS]\033[0m Error reading GPS." << endl;
					case 4:
						cout << "\033[31m[WAYPTS]\033[0m Manual mode engaged." << endl;
					break;
				}
				cout << "\033[1;33m[WAYPTS]\033[0m In state "		<< state << "."	<< endl;
				cout << "\033[1;33m[WAYPTS]\033[0m Facing " << setprecision(6) << yaw << ", at coordinates (" << currentCoord.lat << ", " <<	currentCoord.lon << ")" << endl;

				cout << "\033[1;33m[WAYPTS]\033[0m The next waypoint is at (" << setprecision(6) << waypoints_list[wp_it].lat << ", " << waypoints_list[wp_it].lon << ")"	<< endl;
				
				cout << "\033[1;33m[WAYPTS]\033[0m It is " << setprecision(7) << distanceToNextWaypoint	<< "m away, at a bearing of " << bearingToNextWaypoint << "." << endl;
			
				printFB_Data(&stop);
				
				pastState = state;
			}

			switch(state) {
				case 0:													//Case 0:	Not in auto mode, standby
					fb.setFB_Data(&stop);									//Stop moving
					
					logs.writeLogLine("\033[1;32m[WAYPTS]\033[0m Manual mode");
					sprintf(str_buf, "[WAYPTS] Currently at %f %f.", currentCoord.lat, currentCoord.lon);
					logs.writeLogLine(str_buf);
					
					break;
				
				case 1:
					bearingToNextWaypoint = calculate_bearing(currentCoord, waypoints_list[wp_it]);	//Set a Course
					setCourse(&course, distanceToNextWaypoint, bearingToNextWaypoint, yaw);
					fb.setFB_Data(&course);																//Give command to flight boars
					
					sprintf(str_buf, "[WAYPTS] Aileron is %d, Elevator is %d", course.aileron, course.elevator);
					logs.writeLogLine(str_buf);
					sprintf(str_buf, "[WAYPTS] Moving to next waypoint. It has latitude %f and longitude %f.", waypoints_list[wp_it].lat, waypoints_list[wp_it].lon);
					logs.writeLogLine(str_buf);
					sprintf(str_buf, "[WAYPTS] Currently at %f %f, moving %f m at a bearing of %f degrees.", currentCoord.lat, currentCoord.lon, distanceToNextWaypoint, bearingToNextWaypoint);
					logs.writeLogLine(str_buf);

					break;
				
				case 2:
					fb.setFB_Data(&stop);
					buzzer.playBuzzer(0.4, 100, 100);
					
					logs.writeLogLine("\033[1;32m[WAYPTS]\033[0m Reached waypoint, stopping");
					
					//waypoints_list.push_back(waypoints_list.front());
					wp_it++;
					//waypoints_list.pop_front();

					boost::this_thread::sleep(boost::posix_time::milliseconds(WAIT_AT_WAYPOINTS));
					
					cout << "\033[1;32m[WAYPTS]\033[0m Moving to next waypoint." << endl;
					break;
				
				case 3:
				default:
					fb.setFB_Data(&stop);
					
					logs.writeLogLine("\033[31m[WAYPTS]\033[0m Error reading GPS, stopping");
				break;
			}
			boost::this_thread::sleep(boost::posix_time::milliseconds(100));

		}
	} catch (...) {
		cout << "\033[31m[WAYPTS]\033[0m Error encountered. Stopping copter." << endl;
		fb.setFB_Data(&stop);
		printFB_Data(&stop);
		state = 3;
	}
	cout << "\033[1;32m[WAYPTS]\033[0m Waypoints flight loop terminating." << endl;
	
	buzzer.playBuzzer(0.2, 100, 100);
	usleep(400000);
	buzzer.playBuzzer(0.2, 100, 100);
	usleep(400000);
	buzzer.playBuzzer(0.2, 100, 100);
	usleep(400000);
}
コード例 #6
0
ファイル: waypoints_loop4.cpp プロジェクト: picopter/picopter
/*
 *	flightLoop
 *		1) Initialise copter systems
 *		2) Wait for user allowance to fly.
 *		3) Read in list of waypoints
 *		4) Fly to first waypoint, wait, fly to next, etc..
 *		5) If the end is reached, stop.
 *		
 *		The user may stop the flight at any time. It will continue if the user resumes. At any
 *		point, the user may stop the current flight path and change the waypoint configuration. Upon
 *		resuming flight, the copter will read in the new list of waypoints and start at the
 *		beginning.
 */
void waypoints_loop4(hardware &hardware_list, Logger &log, deque<coord> &waypoints_list, string config_filename) {	
	cout << "\033[1;32m[WAYPTS]\033[0m Waypoints thread initiated, travelling to the following waypoints:" << endl;
	char str_buf[BUFSIZ];
	log.clearLog();
	
	time_t start, now, last_displayed;
	time(&start);
    time(&now);
    time(&last_displayed);
	
	//Grab references to hardware
	FlightBoard *fb	= hardware_list.fb;
	GPS *gps		= hardware_list.gps;
	IMU *imu		= hardware_list.imu;
	
	bool useimu = hardware_list.IMU_Working;
	double yaw = 0;
	
	//Configure configurable variables (if file is given)
	if(!config_filename.empty()) {
		loadParameters(config_filename);
	}
	
	//Construct PID controller
	PID controller_perp   = PID(-Kp_PERP, -Ki_PERP, -Kd_PERP, MAIN_LOOP_DELAY, 3, 0.95);
	PID controller_inline = PID(-Kp_INLINE, -Ki_INLINE, -Kd_INLINE, MAIN_LOOP_DELAY, 3, 0.95);
	
	//Construct buzzer
	Buzzer buzzer;
	buzzer.playBuzzer(BUZZER_DURATION, BUZZER_FREQUENCY, BUZZER_VOLUME);
	usleep((int)(BUZZER_DURATION*1.1)*1000*1000);
	
	//Print list of waypoints
	for(size_t i = 0; i != waypoints_list.size(); i++) {
		cout << "         " << i+1 << ": (" << setprecision(6) << waypoints_list[i].lat << ", " << setprecision(6) << waypoints_list[i].lon << ")" << endl;
	}
	
	//Wait here for auto mode and conduct bearing test if necessary
	state = 6;
    cout << "\033[1;33m[WAYPTS]\033[0m Waiting for auto mode." << endl;
	while ( !gpio::isAutoMode() && !exitProgram) usleep(500*1000);
    cout << "\033[1;31m[WAYPTS]\033[0m Auto mode engaged." << endl;
	
	if (!useimu && !exitProgram) {
		buzzer.playBuzzer(0.25, 10, 100);
        cout << "\033[1;31m[WAYPTS]\033[0m Conducting bearing test..." << endl;
		state = 5;
		yaw = inferBearing(fb, gps);
        cout << "\033[1;31m[WAYPTS]\033[0m Bearing test complete." << endl;
	}
	state = 1;
	
	//Initialise loop variables
	coord		currentCoord = {-1, -1};
	double		distanceToNextWaypoint;
	double		bearingToNextWaypoint;
	FB_Data		course = {0, 0, 0, 0};
	int			pastState = -1;
	velocity 	flightVector = {-1, -1};
	
	//Plot initial path
	currentCoord = getCoord(gps);
	line path;
	if (!waypoints_list.empty()) {
		path = get_path(currentCoord, waypoints_list[wp_it]);
	}

    
    cout << "\033[1;32m[WAYPTS]\033[0m Starting main loop." << endl;
    
	try {	// Check for any errors, and stop the copter.
		while(!exitProgram) {
			currentCoord = getCoord(gps);
			
			
			//Write data for Michael
			time(&now);
			if (!waypoints_list.empty()) {
				sprintf(str_buf, "%.f,%3.6f,%3.6f,%3.6f,%3.6f", difftime(now, start), currentCoord.lat, currentCoord.lon, waypoints_list[wp_it].lat, waypoints_list[wp_it].lon);
			} else {
				sprintf(str_buf, "%.f,%3.6f,%3.6f,,", difftime(now, start), currentCoord.lat, currentCoord.lon);
			}
			log.writeLogLine(str_buf, false);
			
			
			if(!waypoints_list.empty()) {
				distanceToNextWaypoint = calculate_distance(currentCoord, waypoints_list[wp_it]);
				bearingToNextWaypoint = calculate_bearing(currentCoord, waypoints_list[wp_it]);
			}
			
			if (useimu) yaw = getYaw(imu);
			

			
			
			
			
			/* State 4: Manual mode. */
			if (!gpio::isAutoMode()) {
				state = 4;
				wp_it = 0;
				exitProgram = true;
			/* State 0: All stop */
			} else if (waypoints_list.empty() || wp_it == waypoints_list.size() || userState == 0 ) {
				state = 11;
				userState = 0;
				wp_it = 0;
				exitProgram = true;
			/* State 3: Error. */
			} else if (state == 3 || !checkInPerth(&currentCoord)) {
				state = 3;
				exitProgram = true;
			/* State 2: At waypoint. */
			} else if (distanceToNextWaypoint < WAYPOINT_RADIUS) {
				state = 2;
			/* State 1: Travelling to waypoint. */
			} else {
				state = 1;
			}
			
			/* Only give output if the state changes. Less spamey.. */
			if (pastState != state || (state == 1 && difftime(now, last_displayed) >0.9)) {
				switch (state) {
					case 0:
						cout << "\033[1;32m[WAYPTS]\033[0m All stop." << endl;
						break;
					case 1:
						cout << "\033[1;32m[WAYPTS]\033[0m Travelling to waypoint " << wp_it << ", at (" << waypoints_list[wp_it].lat << "," << waypoints_list[wp_it].lon << ")" << endl;
						break;
					case 2:
						cout << "\033[1;32m[WAYPTS]\033[0m At waypoint" << wp_it << "." << endl;
						break;
					case 3:
						cout << "\033[31m[WAYPTS]\033[0m Error reading GPS." << endl;
					case 4:
						cout << "\033[31m[WAYPTS]\033[0m Manual mode engaged." << endl;
					break;
				}
				cout << "\033[1;33m[WAYPTS]\033[0m In state "		<< state << "."	<< endl;
				cout << "\033[1;33m[WAYPTS]\033[0m Facing " << setprecision(6) << yaw << ", at coordinates (" << currentCoord.lat << ", " <<	currentCoord.lon << ")" << endl;

				cout << "\033[1;33m[WAYPTS]\033[0m The next waypoint is at (" << setprecision(6) << waypoints_list[wp_it].lat << ", " << waypoints_list[wp_it].lon << ")"	<< endl;
				
				cout << "\033[1;33m[WAYPTS]\033[0m It is " << setprecision(7) << distanceToNextWaypoint	<< "m away, at a bearing of " << bearingToNextWaypoint << "." << endl;
				
				pastState = state;
				last_displayed = now;

			}

			switch(state) {
				case 0:													//Case 0:	Not in auto mode, standby
					fb->setFB_Data(&stop);									//Stop moving
					
					/*
					log.writeLogLine("\033[1;32m[WAYPTS]\033[0m Manual mode");
					sprintf(str_buf, "[WAYPTS] Currently at %f %f.", currentCoord.lat, currentCoord.lon);
					log.writeLogLine(str_buf);
					*/
					
					break;
				
				case 1:
				
					flightVector = get_velocity(&controller_perp, &controller_inline, currentCoord, path, SPEED_LIMIT_);
					setCourse(&course, flightVector, yaw);
					fb->setFB_Data(&course);																//Give command to flight boars
					
					/*
					sprintf(str_buf, "[WAYPTS] Aileron is %d, Elevator is %d", course.aileron, course.elevator);
					log.writeLogLine(str_buf);
					sprintf(str_buf, "[WAYPTS] Moving to next waypoint. It has latitude %f and longitude %f.", waypoints_list[wp_it].lat, waypoints_list[wp_it].lon);
					log.writeLogLine(str_buf);
					sprintf(str_buf, "[WAYPTS] Currently at %f %f, moving %f m at a bearing of %f degrees.", currentCoord.lat, currentCoord.lon, distanceToNextWaypoint, bearingToNextWaypoint);
					log.writeLogLine(str_buf);
					*/

					break;
				
				case 2:
					fb->setFB_Data(&stop);
					buzzer.playBuzzer(BUZZER_DURATION, BUZZER_FREQUENCY, BUZZER_VOLUME);
					
					/*
					log.writeLogLine("\033[1;32m[WAYPTS]\033[0m Reached waypoint, stopping");
					*/
					
					
					wp_it++;
					if(repeatLoop && wp_it == waypoints_list.size()) {
						wp_it = 0;
					}
					
					if (wp_it != waypoints_list.size()) {
						path = get_path(currentCoord, waypoints_list[wp_it]);
					}
					
					usleep(WAIT_AT_WAYPOINTS*1000);
					
					controller_perp.clear();
					controller_inline.clear();
					cout << "\033[1;32m[WAYPTS]\033[0m Moving to next waypoint." << endl;
					break;
				
				case 3:
				default:
					fb->setFB_Data(&stop);
					/*
					log.writeLogLine("\033[31m[WAYPTS]\033[0m Error reading GPS, stopping");
					*/
				break;
			}
			usleep(MAIN_LOOP_DELAY*1000);

		}
	} catch (...) {
		cout << "\033[31m[WAYPTS]\033[0m Error encountered. Stopping copter." << endl;
		fb->setFB_Data(&stop);
		state = 3;
	}
	cout << "\033[1;32m[WAYPTS]\033[0m Waypoints flight loop terminating." << endl;
	
	buzzer.playBuzzer(1.0, 20, 60);
	usleep(2100*1000);
}