int main(void)
{
Systick_Init();
while (1)
{
gps.update();
if(gps.fix())
{
latitude = gps.latitude();
longitude = gps.longitude();
time = gps.time();
date = gps.date();
}
}
}
void isGPSaccessible(notification_data* data)
{
GPS pGPS = gps_get_instance();
bool b = pGPS->isAccessible(pGPS);
sprintf(data->result_text,"%s",b?"accessible":"not accessible");
}
void onGPSdisconnect(notification_data* data)
{
GPS pGPS = gps_get_instance();
bool b=pGPS->onDisconnect(pGPS);
sprintf(data->result_text,"%s",b?"disconnected":"not disconnected");
}
void loop(){
if ( ((millis() - timer) >= (unsigned int)DELAY_SEND) && (refreshed) ){
refreshed = false;
gps.getTrame();
press.getTrame();
pressext.getTrame();
temp.getTrame();
pile.getTrame();
chamallow.getTrame();
conduct.getTrame();
ph.getTrame();
co2.getTrame();
timer = millis();
} else if ( ((millis() - timer) >= (unsigned int)DELAY_REFRESH) && (!(refreshed)) ) {
press.refresh();
pressext.refresh();
temp.refresh();
pile.refresh();
conduct.refresh();
chamallow.refresh();
co2.refresh();
refreshed = true;
}
if (Serial1.available() > 0){
gps.refresh();
}
if (Serial2.available() > 0) {
ph.refresh();
}
}
void * GPS::GPSMainThread(void *arg)
{
GPS *gps = (GPS*)arg;
if(gps == NULL)
cerr << "UNABLE TO ATTACH GPS OBJECT" << endl;
gps->SetupUART();
cout << "here" << endl;
while(!gps->shutDown)
{
if(gps->Rx())
{
for(int i=0;i<gps->bufferCount;i++)
{
if(gps->tinyGPS.encode(gps->rx_buffer[i]))
{
gps->GetGPS();
}
}
gps->bufferBlocked = false;
}
}
return NULL;
}
// Called the first time that a client tries to kick the GPS to update.
//
// We detect the real GPS, then update the pointer we have been called through
// and return.
//
bool
AP_GPS_Auto::read(void)
{
static uint32_t last_baud_change_ms;
static uint8_t last_baud;
GPS *gps;
uint32_t now = hal.scheduler->millis();
if (now - last_baud_change_ms > 1200) {
// its been more than 1.2 seconds without detection on this
// GPS - switch to another baud rate
_port->begin(pgm_read_dword(&baudrates[last_baud]), 256, 16);
last_baud++;
last_baud_change_ms = now;
if (last_baud == sizeof(baudrates) / sizeof(baudrates[0])) {
last_baud = 0;
}
// write config strings for the types of GPS we support
_send_progstr(_port, _mtk_set_binary, sizeof(_mtk_set_binary));
_send_progstr(_port, AP_GPS_UBLOX::_ublox_set_binary, AP_GPS_UBLOX::_ublox_set_binary_size);
_send_progstr(_port, _sirf_set_binary, sizeof(_sirf_set_binary));
}
_update_progstr();
if (NULL != (gps = _detect())) {
// configure the detected GPS
gps->init(_port, _nav_setting);
hal.console->println_P(PSTR("OK"));
*_gps = gps;
return true;
}
return false;
}
void GPSrecv(notification_data* data)
{
GPS pGPS = gps_get_instance();
Location l = pGPS->Recv(pGPS);
if(l.validation == true)
sprintf(data->result_text,"latitude : %lf<br>longitude : %lf",l.latitude, l.longitude);
else
sprintf(data->result_text,"Recv Fail try Again");
}
void GPS::updateGps() {
gpsData.idlecount++;
if(gpsData.idlecount == 200) {
//Serial.println("NO data");
gpsData.idlecount = 0;
}
// Serial.println(Serial2.available());
while (Serial2.available()) {
unsigned char c = Serial2.read();
int ret=0;
if (gpsData.state == GPS_DETECTING) {
ret = rec.ubloxProcessData(c);
if (ret) {
// found GPS device start sending configuration
gpsConfigsSent = 0;
gpsConfigTimer = 1;
break;
}
}
else {
// Normal operation just execute the detected parser
ret = rec.ubloxProcessData(c);
}
if (gpsConfigTimer) {
if (gpsConfigTimer==1) {
gpsSendConfig();
}
gpsConfigTimer--;
}
// Upon a successfully parsed sentence, zero the idlecounter and update position data
if (ret) {
if (gpsData.state == GPS_DETECTING) {
gpsData.state = GPS_NOFIX; // make sure to lose detecting state (state may not have been updated by parser)
}
gpsData.idlecount=0;
currentPosition.latitude=gpsData.lat;
currentPosition.longitude=gpsData.lon;
currentPosition.altitude=gpsData.height;
Serial.print("Latitutde = ");Serial.print(currentPosition.latitude);Serial.print(" Longitude = ");Serial.print(currentPosition.longitude);Serial.print(" Altitude = ");Serial.println(currentPosition.altitude);
}
}
// Schedule confg sending if needed
if (gpsConfigTimer) {
if (gpsConfigTimer==1) {
gpsSendConfig();
}
gpsConfigTimer--;
}
}
int main(int argc, char *argv[])
{
QApplication a(argc, argv);
MainWindow w;
PCSCResourceManager manager;
Flowmeter flowmeter;
RealtimeRunningData runningData(&g_gps, &flowmeter);
RunningDataUploader uploader(&runningData, g_settings.ServerIP(), g_settings.ServerPort());
BInputMethod inputMethod;
QWSServer::setCurrentInputMethod(&inputMethod);
g_gps.start();
w.setDriver(&g_driver);
w.show();
g_driver.connectPCSCResourceManager(&manager);
g_warehouseProxy.setAttachedDriver(&g_driver);
manager.start();
runningData.start();
uploader.start();
return a.exec();
}
int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
{
GPS *gps = (GPS *)user;
switch (type) {
case GPSCallbackType::readDeviceData: {
int num_read = gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1));
if (num_read > 0 && gps->_dump_from_gps_device_fd >= 0) {
if (write(gps->_dump_from_gps_device_fd, data1, (size_t)num_read) != (size_t)num_read) {
PX4_WARN("gps dump failed");
}
}
return num_read;
}
case GPSCallbackType::writeDeviceData:
if (gps->_dump_to_gps_device_fd >= 0) {
if (write(gps->_dump_to_gps_device_fd, data1, (size_t)data2) != (size_t)data2) {
PX4_WARN("gps dump failed");
}
}
return write(gps->_serial_fd, data1, (size_t)data2);
case GPSCallbackType::setBaudrate:
return gps->setBaudrate(data2);
case GPSCallbackType::gotRTCMMessage:
/* not used */
break;
case GPSCallbackType::surveyInStatus:
/* not used */
break;
case GPSCallbackType::setClock:
px4_clock_settime(CLOCK_REALTIME, (timespec *)data1);
break;
}
return 0;
}
/*
* 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();
}
// Called the first time that a client tries to kick the GPS to update.
//
// We detect the real GPS, then update the pointer we have been called through
// and return.
//
/// @todo This routine spends a long time trying to detect a GPS. That's not strictly
/// desirable; it might be a good idea to rethink the logic here to make it
/// more asynchronous, so that other parts of the system can get a chance
/// to run while GPS detection is in progress.
///
bool
AP_GPS_Auto::read(void)
{
GPS *gps;
uint8_t i;
uint32_t then;
// Loop through possible baudrates trying to detect a GPS at one of them.
//
// Note that we need to have a FastSerial rather than a Stream here because
// Stream has no idea of line speeds. FastSerial is quite OK with us calling
// ::begin any number of times.
//
for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
_fs->begin(baudrates[i]);
if (NULL != (gps = _detect())) {
// configure the detected GPS and give it a chance to listen to its device
gps->init();
then = millis();
while ((millis() - then) < 1200) {
// if we get a successful update from the GPS, we are done
gps->new_data = false;
gps->update();
if (gps->new_data) {
Serial.println_P(PSTR("OK"));
Serial.print("GPS:");
Serial.println(baudrates[i]);
*_gps = gps;
return true;
}
}
// GPS driver failed to parse any data from GPS,
// delete the driver and continue the process.
Serial.println_P(PSTR("failed, retrying"));
delete gps;
}
}
return false;
}
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 GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
{
GPS *gps = (GPS *)user;
switch (type) {
case GPSCallbackType::readDeviceData: {
int num_read = gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1));
if (num_read > 0) {
gps->dumpGpsData((uint8_t *)data1, (size_t)num_read, false);
}
return num_read;
}
case GPSCallbackType::writeDeviceData:
gps->dumpGpsData((uint8_t *)data1, (size_t)data2, true);
return write(gps->_serial_fd, data1, (size_t)data2);
case GPSCallbackType::setBaudrate:
return gps->setBaudrate(data2);
case GPSCallbackType::gotRTCMMessage:
/* not used */
break;
case GPSCallbackType::surveyInStatus:
/* not used */
break;
case GPSCallbackType::setClock:
px4_clock_settime(CLOCK_REALTIME, (timespec *)data1);
break;
}
return 0;
}
int main(int argc, char ** argv, char ** envp)
{
GPS * g = new GPS(100, 0x55);
g->print();
ALT_GPS * a = new ALT_GPS(1, 2, 3);
a->print2();
GPS * fred = NULL;
int choice;
cin >> choice;
switch (choice)
{
case 1:
fred = g;
break;
case 2:
fred = a;
break;
}
fred->print2();
}
int main(int argc, char *argv[])
{
string mission_file;
string run_command = argv[0];
xmldoc::MOOSAppDocumentation documentation(argv[0]);
for(int i=1; i<argc; i++) {
string argi = argv[i];
if((argi=="-v") || (argi=="--version") || (argi=="-version"))
documentation.showReleaseInfoAndExit();
else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
documentation.showExampleConfigAndExit();
else if((argi == "-h") || (argi == "--help") || (argi=="-help"))
documentation.showHelpAndExit();
else if((argi == "-i") || (argi == "--interface"))
documentation.showInterfaceAndExit();
else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
mission_file = argv[i];
else if(strBegins(argi, "--alias="))
run_command = argi.substr(8);
else if(i==2)
run_command = argi;
}
if(mission_file == "")
documentation.showHelpAndExit();
cout << termColor("green");
cout << "iGPS launching as " << run_command << endl;
cout << termColor() << endl;
GPS GPS;
GPS.Run(run_command.c_str(), mission_file.c_str());
return(0);
}
void setup() {
pinMode((byte)10, OUTPUT);
pinMode((byte)13, OUTPUT);
so.init();
sd.init();
press.init();
pressext.init();
gps.init();
temp.init();
conduct.init();
chamallow.init();
ph.init();
co2.init();
press.addOut(&so);
pressext.addOut(&so);
temp.addOut(&so);
gps.addOut(&so);
pile.addOut(&so);
conduct.addOut(&so);
chamallow.addOut(&so);
ph.addOut(&so);
co2.addOut(&so);
press.addOut(&sd);
pressext.addOut(&sd);
temp.addOut(&sd);
gps.addOut(&sd);
pile.addOut(&sd);
conduct.addOut(&sd);
chamallow.addOut(&sd);
ph.addOut(&sd);
co2.addOut(&sd);
}
/*
* 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);
}
void test()
{
typedef typename GPS::Traits_2 Traits;
typedef typename Traits::Point_2 Point_2;
typedef typename Traits::Polygon_2 Polygon_2;
typedef typename Traits::Polygon_with_holes_2 Polygon_with_holes_2;
Polygon_2 pgn1, pgn2;
Polygon_with_holes_2 pgn_with_holes1, pgn_with_holes2;
std::vector<Polygon_2> polygons;
std::vector<Polygon_with_holes_2> polygons_with_holes;
GPS gps;
GPS other;
gps.intersection(pgn1);
gps.intersection(pgn_with_holes1);
gps.intersection(gps);
gps.join(pgn1);
gps.join(pgn_with_holes1);
gps.join(gps);
gps.difference(pgn1);
gps.difference(pgn_with_holes1);
gps.difference(gps);
gps.symmetric_difference(pgn1);
gps.symmetric_difference(pgn_with_holes1);
gps.symmetric_difference(gps);
gps.intersection(polygons.begin(), polygons.end());
gps.intersection(polygons_with_holes.begin(), polygons_with_holes.end());
gps.intersection(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end());
gps.join(polygons.begin(), polygons.end());
gps.join(polygons_with_holes.begin(), polygons_with_holes.end());
gps.join(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end());
gps.symmetric_difference(polygons.begin(), polygons.end());
gps.symmetric_difference(polygons_with_holes.begin(), polygons_with_holes.end());
gps.symmetric_difference(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end());
gps.do_intersect(pgn1);
gps.do_intersect(pgn_with_holes1);
gps.do_intersect(other);
gps.complement();
gps.complement(gps);
gps.do_intersect(polygons.begin(), polygons.end());
gps.do_intersect(polygons_with_holes.begin(), polygons_with_holes.end());
gps.do_intersect(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end());
std::vector<Polygon_with_holes_2> result;
Traits tr;
// global functions
CGAL::do_intersect(pgn1, pgn2);
CGAL::do_intersect(pgn1, pgn_with_holes2);
CGAL::do_intersect(pgn_with_holes1, pgn2);
CGAL::do_intersect(pgn_with_holes1, pgn_with_holes2);
CGAL::do_intersect(polygons.begin(), polygons.end());
CGAL::do_intersect(polygons_with_holes.begin(), polygons_with_holes.end());
CGAL::do_intersect(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end());
CGAL::do_intersect(pgn1, pgn2, tr);
CGAL::do_intersect(pgn1, pgn_with_holes2, tr);
CGAL::do_intersect(pgn_with_holes1, pgn2, tr);
CGAL::do_intersect(pgn_with_holes1, pgn_with_holes2, tr);
CGAL::do_intersect(pgn_with_holes1, pgn_with_holes2);
CGAL::do_intersect(polygons.begin(), polygons.end(), tr);
CGAL::do_intersect(polygons_with_holes.begin(), polygons_with_holes.end(), tr);
CGAL::do_intersect(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end(), tr);
CGAL::intersection(pgn1, pgn2, std::back_inserter(result));
CGAL::intersection(pgn1, pgn_with_holes2, std::back_inserter(result));
CGAL::intersection(pgn_with_holes1, pgn2, std::back_inserter(result));
CGAL::intersection(pgn_with_holes1, pgn_with_holes2, std::back_inserter(result));
CGAL::intersection(polygons.begin(), polygons.end(), std::back_inserter(result));
CGAL::intersection(polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result));
CGAL::intersection(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result));
CGAL::intersection(pgn1, pgn2, std::back_inserter(result), tr);
CGAL::intersection(pgn1, pgn_with_holes2, std::back_inserter(result), tr);
CGAL::intersection(pgn_with_holes1, pgn2, std::back_inserter(result), tr);
CGAL::intersection(pgn_with_holes1, pgn_with_holes2, std::back_inserter(result), tr);
CGAL::intersection(polygons.begin(), polygons.end(), std::back_inserter(result), tr);
CGAL::intersection(polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result), tr);
CGAL::intersection(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result), tr);
Polygon_with_holes_2 res;
CGAL::join(pgn1, pgn2, res);
CGAL::join(pgn1, pgn_with_holes2, res);
CGAL::join(pgn_with_holes1, pgn2, res);
CGAL::join(pgn_with_holes1, pgn_with_holes2, res);
CGAL::join(polygons.begin(), polygons.end(), std::back_inserter(result));
CGAL::join(polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result));
CGAL::join(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result));
CGAL::join(pgn1, pgn2, res, tr);
CGAL::join(pgn1, pgn_with_holes2, res, tr);
CGAL::join(pgn_with_holes1, pgn2, res, tr);
CGAL::join(pgn_with_holes1, pgn_with_holes2, res, tr);
CGAL::join(polygons.begin(), polygons.end(), std::back_inserter(result), tr);
CGAL::join(polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result), tr);
CGAL::join(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result), tr);
CGAL::difference(pgn1, pgn2, std::back_inserter(result));
CGAL::difference(pgn1, pgn_with_holes2, std::back_inserter(result));
CGAL::difference(pgn_with_holes1, pgn2, std::back_inserter(result));
CGAL::difference(pgn_with_holes1, pgn_with_holes2, std::back_inserter(result));
CGAL::difference(pgn1, pgn2, std::back_inserter(result), tr);
CGAL::difference(pgn1, pgn_with_holes2, std::back_inserter(result), tr);
CGAL::difference(pgn_with_holes1, pgn2, std::back_inserter(result), tr);
CGAL::difference(pgn_with_holes1, pgn_with_holes2, std::back_inserter(result), tr);
CGAL::symmetric_difference(pgn1, pgn2, std::back_inserter(result));
CGAL::symmetric_difference(pgn1, pgn_with_holes2, std::back_inserter(result));
CGAL::symmetric_difference(pgn_with_holes1, pgn2, std::back_inserter(result));
CGAL::symmetric_difference(pgn_with_holes1, pgn_with_holes2, std::back_inserter(result));
CGAL::symmetric_difference(polygons.begin(), polygons.end(), std::back_inserter(result));
CGAL::symmetric_difference(polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result));
CGAL::symmetric_difference(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result));
CGAL::symmetric_difference(pgn1, pgn2, std::back_inserter(result), tr);
CGAL::symmetric_difference(pgn1, pgn_with_holes2, std::back_inserter(result), tr);
CGAL::symmetric_difference(pgn_with_holes1, pgn2, std::back_inserter(result), tr);
CGAL::symmetric_difference(pgn_with_holes1, pgn_with_holes2, std::back_inserter(result), tr);
CGAL::symmetric_difference(polygons.begin(), polygons.end(), std::back_inserter(result), tr);
CGAL::symmetric_difference(polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result), tr);
CGAL::symmetric_difference(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end(), std::back_inserter(result), tr);
Polygon_with_holes_2 res2;
CGAL::complement(pgn1, res2);
CGAL::complement(pgn_with_holes1, std::back_inserter(result));
CGAL::complement(pgn1, res2, tr);
CGAL::complement(pgn_with_holes1, std::back_inserter(result), tr);
GPS gps2(pgn1);
GPS gps3(pgn_with_holes1);
GPS gps4;
gps4.insert(pgn1);
gps4.insert(pgn_with_holes1);
gps4.insert(polygons.begin(), polygons.end());
gps4.insert(polygons.begin(), polygons.end(),
polygons_with_holes.begin(), polygons_with_holes.end());
gps.complement(gps2);
gps.intersection(gps2, gps3);
gps.join(gps2, gps3);
gps.difference(gps2, gps3);
gps.symmetric_difference(gps2, gps3);
Point_2 pt;
gps.oriented_side(pt);
gps.oriented_side(pgn1);
gps.oriented_side(pgn_with_holes1);
gps.oriented_side(pgn_with_holes1);
gps.oriented_side(gps);
gps.locate(pt, pgn_with_holes1);
GPS new_gps(gps);
GPS new_gps2 = gps;
}
int GPS::ubloxProcessData(unsigned char data) {
int parsed = 0;
switch (ubloxProcessDataState) {
case WAIT_SYNC1:
if (data == 0xb5) {
ubloxProcessDataState = WAIT_SYNC2;
}
break;
case WAIT_SYNC2:
if (data == 0x62) {
ubloxProcessDataState = GET_CLASS;
}
else if (data == 0xb5) {
// ubloxProcessDataState = GET_SYNC2;
}
else {
ubloxProcessDataState = WAIT_SYNC1;
}
break;
case GET_CLASS:
ubloxClass=data;
ubloxCKA=data;
ubloxCKB=data;
ubloxProcessDataState = GET_ID;
break;
case GET_ID:
ubloxId=data;
ubloxCKA += data;
ubloxCKB += ubloxCKA;
ubloxProcessDataState = GET_LL;
break;
case GET_LL:
ubloxExpectedDataLength = data;
ubloxCKA += data;
ubloxCKB += ubloxCKA;
ubloxProcessDataState = GET_LH;
break;
case GET_LH:
ubloxExpectedDataLength += data << 8;
ubloxDataLength=0;
ubloxCKA += data;
ubloxCKB += ubloxCKA;
if (ubloxExpectedDataLength <= sizeof(ubloxMessage)) {
ubloxProcessDataState = GET_DATA;
}
else {
// discard overlong message
ubloxProcessDataState = WAIT_SYNC1;
}
break;
case GET_DATA:
ubloxCKA += data;
ubloxCKB += ubloxCKA;
// next will discard data if it exceeds our biggest known msg
if (ubloxDataLength < sizeof(ubloxMessage)) {
ubloxMessage.raw[ubloxDataLength++] = data;
}
if (ubloxDataLength >= ubloxExpectedDataLength) {
ubloxProcessDataState = GET_CKA;
}
break;
case GET_CKA:
if (ubloxCKA != data) {
ubloxProcessDataState = WAIT_SYNC1;
}
else {
ubloxProcessDataState = GET_CKB;
}
break;
case GET_CKB:
if (ubloxCKB == data) {
parsed = 1;
//Serial.print("parsed ");Serial.println(parsed);
rec.ubloxParseData();
}
ubloxProcessDataState = WAIT_SYNC1;
break;
}
return parsed;
}
void GPS::initializeGps() {
gpsData.baudrate = 2;
Serial2.begin(gpsBaudRates[gpsData.baudrate]);
rec.ubloxInit();
rec.initializeGpsData();
}
