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