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