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