예제 #1
0
/*
 *	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);
}
예제 #2
0
		webInterfaceHandler() {
			cout << "\033[36m[THRIFT]\033[0m Initialising hexacopter systems." << endl;
			if (!initialise(&fb, &gps, &imu, &cam)) terminate();
			buzzer.playBuzzer(0.4, 100, 100);
		}
예제 #3
0
/*
 *	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);
}