/* * 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(); }
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; }
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(); }
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, ¶meters, 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; }
/* * 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); }
/* * 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); }