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