Exemplo n.º 1
0
void loop()
{
	p_myGPSData = &myGPSData;

	readGPS(p_myGPSData);

	CardReaderValue.myFilePointer.print(myGPSData.hour); CardReaderValue.myFilePointer.print("h ");
	CardReaderValue.myFilePointer.print(myGPSData.minute); CardReaderValue.myFilePointer.print("m ");
	CardReaderValue.myFilePointer.print(myGPSData.seconds); CardReaderValue.myFilePointer.print("s ");
	CardReaderValue.myFilePointer.print(myGPSData.milliseconds); CardReaderValue.myFilePointer.print("ms\n");
	CardReaderValue.myFilePointer.print(myGPSData.day); CardReaderValue.myFilePointer.print(".");
	CardReaderValue.myFilePointer.print(myGPSData.month); CardReaderValue.myFilePointer.print(".");
	CardReaderValue.myFilePointer.print(myGPSData.year);

	CardReaderValue.myFilePointer.print("fix: "); CardReaderValue.myFilePointer.print(myGPSData.fix);
	CardReaderValue.myFilePointer.print(" qualety: "); CardReaderValue.myFilePointer.print(myGPSData.fixquality);
	CardReaderValue.myFilePointer.print("\n");


	CardReaderValue.myFilePointer.print("lat: ");  CardReaderValue.myFilePointer.print(myGPSData.latitude);
	CardReaderValue.myFilePointer.print("\n");

	CardReaderValue.myFilePointer.print("lon: "); CardReaderValue.myFilePointer.print(myGPSData.longitude);
	CardReaderValue.myFilePointer.print("\n");

	CardReaderValue.myFilePointer.print("Speed: "); CardReaderValue.myFilePointer.print(myGPSData.speed);
	CardReaderValue.myFilePointer.print("\n");

	CardReaderValue.myFilePointer.print("angle: "); CardReaderValue.myFilePointer.print(myGPSData.angle);
	CardReaderValue.myFilePointer.print("\n");

	CardReaderValue.myFilePointer.print("höhe: "); CardReaderValue.myFilePointer.print(myGPSData.altitude);
	CardReaderValue.myFilePointer.print("\n");

	CardReaderValue.myFilePointer.print("satellites: "); CardReaderValue.myFilePointer.print(myGPSData.satellites);
	CardReaderValue.myFilePointer.print("\n");



	Serial.print(myGPSData.hour); Serial.print("h ");
	Serial.print(myGPSData.minute); Serial.print("m ");
	Serial.print(myGPSData.seconds); Serial.print("s ");
	Serial.print(myGPSData.milliseconds); Serial.println("ms ");
	Serial.print(myGPSData.day); Serial.print(".");
	Serial.print(myGPSData.month); Serial.print(".");
	Serial.print(myGPSData.year);

	Serial.print("fix: "); Serial.print(myGPSData.fix);
	Serial.print(" qualety: "); Serial.println(myGPSData.fixquality);

	Serial.print("lat: ");  Serial.println(myGPSData.latitude);
	Serial.print("lon: "); Serial.println(myGPSData.longitude);
	Serial.print("Speed: "); Serial.println(myGPSData.speed);
	Serial.print("angle: "); Serial.println(myGPSData.angle);
	Serial.print("höhe: "); Serial.println(myGPSData.altitude);
	Serial.print("satellites: "); Serial.println(myGPSData.satellites);
	Serial.println("");

	Sleep(2000);
}
Exemplo n.º 2
0
int main(int argc, char *argv[]){
  char buffer[76];
  char output[76];
  int count = 0;

  while ( count < 50 && strlen(output) < 75 ){
    readGPS(buffer,"/dev/tty01");
    strcat(output,buffer);
    count++;
  }
  printf("%s\n",output);
  return 0;
}
Exemplo n.º 3
0
void loop()
{
	if(deviceStatus == STOPPED)
	{
		if(LCD.getTouchActivity() == TOUCH_UP)
		{
			LCD.getTouchXY(x, y);
			if(oneMinuteButton.isClicked(x, y))
			{
				pumpEndMillis = millis() + 300;
				digitalWrite(VALVE_CONTROL1, LOW);
				digitalWrite(VALVE_CONTROL2, HIGH);
				initializePolling();
			}
			else if(fiveMinuteButton.isClicked(x, y))
			{
				pumpEndMillis = millis() + 300000;
				digitalWrite(VALVE_CONTROL1, LOW);
				digitalWrite(VALVE_CONTROL2, HIGH)
				initializePolling();
			}
			else if(tenMinuteButton.isClicked(x, y))
			{
				pumpEndMillis = millis() + 600000;
				digitalWrite(VALVE_CONTROL1, LOW);
				digitalWrite(VALVE_CONTROL2, HIGH);
				initializePolling();
			}
		}
	}
	else if(deviceStatus != STOPPED)
	{
		checkInput();
		readGPS();
		checkInput();
		readChip();
		checkInput();
		readTempHumid();
		checkInput();

		if(viewOptions)
		{
			drawOptions();
		}
		
		checkTime();
	}
}
Exemplo n.º 4
0
int main() {
/*        const char *macs[4] = {
                "00-21-29-96-31-e7",
                "00-25-9c-57-de-a2",
                "00-21-91-51-58-6d",
                "00-17-9a-02-4e-05"
        };
*/

	std::string file("../pics/IMG_0024.JPG.log");
	std::string imageName("IMG_0024.JPG");
	LogFileResult result = parseLogFile(file, imageName);

	std::cout << "result found " << result.image_found << " at " << result.image_time << std::endl;
	std::cout << result.aps.size() << " aps." << std::endl;

	for (int i = 0; i < result.aps.size(); i++) {
		AccessPoint point = result.aps[i];
		std::cout << "ap " << point.mac_address << " with signal " << point.signal << " at "  << point.time << std::endl;
	}

        GeoLocateResult gresult = geolocate_locate(result.aps);
	std::cout << "lat " << gresult.lat << " lon " << gresult.lon << " accuracy " <<  gresult.accuracy << " succeeded " << gresult.success << std::endl;

	

	std::string imgFile("photo.jpg");
	dumpAllExif(imgFile);
ExifGPS gps = readGPS(imgFile);

	std::cout << "gps lat " << gps.lat << " gps lon " << gps.lon << std::endl;


	std::string tmpFile("test.jpg");
	writeGPS(tmpFile, gps);

	dumpAllExif(tmpFile);

	return 0;
}
Exemplo n.º 5
0
int main(){
    int i = 0;
    int mapCount = 0, clearMapCount = 0, dumpCount=0;
    int revFrameCount = 0;

#ifdef USE_NORTH
    targetsGPS[maxTargets].lat = ADVANCED5LAT;
    targetsGPS[maxTargets].lon = ADVANCED5LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED6LAT;
    targetsGPS[maxTargets].lon = ADVANCED6LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED7LAT;
    targetsGPS[maxTargets].lon = ADVANCED7LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED8LAT;
    targetsGPS[maxTargets].lon = ADVANCED8LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED2LAT;
    targetsGPS[maxTargets].lon = ADVANCED2LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED1LAT;
    targetsGPS[maxTargets].lon = ADVANCED1LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED3LAT;
    targetsGPS[maxTargets].lon = ADVANCED3LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED12LAT;
    targetsGPS[maxTargets].lon = ADVANCED12LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED4LAT;
    targetsGPS[maxTargets].lon = ADVANCED4LON;
    maxTargets++;
#else
    targetsGPS[maxTargets].lat = ADVANCED4LAT;
    targetsGPS[maxTargets].lon = ADVANCED4LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED1LAT;
    targetsGPS[maxTargets].lon = ADVANCED1LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED2LAT;
    targetsGPS[maxTargets].lon = ADVANCED2LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED3LAT;
    targetsGPS[maxTargets].lon = ADVANCED3LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED11LAT;
    targetsGPS[maxTargets].lon = ADVANCED11LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED8LAT;
    targetsGPS[maxTargets].lon = ADVANCED8LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED7LAT;
    targetsGPS[maxTargets].lon = ADVANCED7LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED6LAT;
    targetsGPS[maxTargets].lon = ADVANCED6LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED11LAT;
    targetsGPS[maxTargets].lon = ADVANCED11LON;
    maxTargets++;
    targetsGPS[maxTargets].lat = ADVANCED5LAT;
    targetsGPS[maxTargets].lon = ADVANCED5LON;
    maxTargets++;
#endif

    maxTargetIndex=maxTargets-1;

    for(i=0;i<maxTargets;i++){// this is converting all GPS point data to XY data.
        targetListXY[i].x = GPSX(targetsGPS[i].lon, startLongitude);
        targetListXY[i].y = GPSY(targetsGPS[i].lat, startLatitude);
    }
    currentXY.x = GPSX(gpsvar.longitude,startLongitude);// converts current robot X location compared to start longitude
    currentXY.y = GPSY(gpsvar.latitude,startLatitude);// converts current robot Y location compared to start latitude

    targetXY = targetListXY[currentTargetIndex];//sets first target GPS point
    nextTargetIndex = (currentTargetIndex + 1)%maxTargets;//sets next target GPS point
    nextXY = targetListXY[nextTargetIndex];// ??
    previousXY.x = GPSX(startLongitude, startLongitude);// why?
    previousXY.y = GPSY(startLatitude, startLatitude);//Why?

    initRoboteq();  /* Initialize roboteq */
    initGuide();//what is guide?
#ifdef USE_VISION // if USE_vision is defined, then initialize vision.
    initVision();
#endif //USE_VISION
#ifdef USE_GPS// if USE_GPS is defined, then initialize GPS.
    initGPS();
    initParser();
#endif //USE_GPS
#ifdef USE_LIDAR// if USE_LIDAR is defined, then initialize LIDAR.
    initObjects();
    initSICK();
#endif //USE_LIDAR
#ifdef DEBUG_VISUALIZER// if defined, then use visualizer.
    initVisualizer();
#endif //DEBUG_VISUALIZER
#ifdef USE_MAP//////>>>>>>>>>>>????
    initMap(0,0,0);
#endif //USE_MAP
#ifdef DUMP_GPS// dump GPS data into file
    FILE *fp;
    fp = fopen("gpsdump.txt", "w");
#endif // DUMP_GPS
    while(1){
        double dir = 1.0;
        double speed = 0.0, turn = 0.0;
        static double turnBoost = 0.750;//Multiplier for turn. Adjust to smooth jerky motions. Usually < 1.0
        static int lSpeed = 0, rSpeed = 0;//Wheel Speed Variables
        if (joystick() != 0) {// is joystick is connected
            if (joy0.buttons & LB_BTN) {// deadman switch, but what does joy0.buttons do?????????????????????????????????
                speed = -joy0.axis[1]; //Up is negative on joystick negate so positive when going forward
                turn = joy0.axis[0];

                lSpeed = (int)((speed + turnBoost*turn)*maxSpeed);//send left motor speed
                rSpeed = (int)((speed - turnBoost*turn)*maxSpeed);//send right motor speed
                }else{ //stop the robot
                     rSpeed=lSpeed=0;
            }
            if(((joy0.buttons & B_BTN)||autoOn)&& (saveImage==0)){//what is the single & ???????????????????
                saveImage =DEBOUNCE_FOR_SAVE_IMAGE;//save each image the camera takes, save image is an int declared in vision_nav.h
            }else{
                if (saveImage) saveImage--; // turn off if button wasn't pressed?
            }
            if(joy0.buttons & RB_BTN){//turn on autonmous mode if start??? button is pressed
                autoOn = 1;
                mode=1;
            }
            if(joy0.buttons & Y_BTN){ // turn off autonomous mode
                autoOn = 0;
                mode =0;
            }
            lastButtons = joy0.buttons;//is this just updating buttons?
        } else{
//            printf("No Joystick Found!\n");
            rSpeed=lSpeed=0;
        }
//
//        printf("3: %f %f\n",BASIC3LAT,BASIC3LON);
//        printf("4: %f %f\n",BASIC4LAT,BASIC4LON);
//        printf("5: %f %f\n",BASIC5LAT,BASIC5LON);
//        getchar();
#ifdef AUTO_SWAP//what is this
        if((currentTargetIndex>1&&targetIndexMem!=currentTargetIndex)||!autoOn||!mode==3){
            startTime=currentTime=(float)(clock()/CLOCKS_PER_SEC);
            targetIndexMem = currentTargetIndex;
        }else{
            currentTime=(float)(clock()/CLOCKS_PER_SEC);
        }
        totalTime = currentTime-startTime;
        if(totalTime>=SWAPTIME&&autoOn){
            swap();
            targetIndexMem = 0;
        }
#endif //AUTO_SWAP

#ifdef USE_GPS
        readGPS();
        currentXY.x = GPSX(gpsvar.longitude,startLongitude);
        currentXY.y = GPSY(gpsvar.latitude,startLatitude);
        robotTheta = ADJUST_RADIANS(DEG2RAD(gpsvar.course));
#else
        currentXY.x = 0.0;
        currentXY.y = 0.0;
        robotTheta = 0.0;
#endif //USE_GPS

        if(autoOn&&!flagPointSet){//this whole thing?????
            flagXY.x=currentXY.x+FLAG_X_ADJUST;
            flagXY.y=currentXY.y;
            flagPointSet=1;
            startAutoTime=currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
        }
        if(autoOn){
            currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
            totalAutoTime = currentAutoTime-startAutoTime;
            if(totalAutoTime>=MODE2DELAY){
                mode1TimeUp=1;//what is mode1 time up?
            }
            printf("TIMEING\n");
        }

//        if(currentTargetIndex <= OPEN_FIELD_INDEX || currentTargetIndex >= maxTargetIndex){
        if(currentTargetIndex <= OPEN_FIELD_INDEX){//if you are on your last target, then set approaching thresh, and dest thresh to larger values?
                //OPEN_FIELD_INDEX is set to 0 above...?
            approachingThresh=4.0;
            destinationThresh=3.0;
        }else{//otherwise set your thresholds to a bit closer.
//            destinationThresh=1.0;
            destinationThresh=0.75;
            approachingThresh=2.5;
        }
//mode1 = lane tracking and obstacle avoidance. mode 2 = vision, lane tracking, but guide to gps. its not primary focus.
//mode3= gps mode in open field, but vision is toned down to not get distracted by random grass.
//mode 4= flag tracking

       if(guide(currentXY, targetXY, previousXY, nextXY, robotTheta, robotWidth, 1)&& !allTargetsReached){//If target reached and and not all targets reached
            printf("REACHED TARGET\n");
            initGuide();// reset PID control stuff. problably resets all control variables.
            previousXY = targetXY;//update last target
            if(currentTargetIndex == maxTargetIndex){ //seeing if you are done with all targets.
                 allTargetsReached = 1;
            }else{//otherwise update all the target information
                currentTargetIndex = (currentTargetIndex + 1);
                nextTargetIndex = (currentTargetIndex + 1)% maxTargets;
                targetXY = targetListXY[currentTargetIndex];
                nextXY = targetListXY[nextTargetIndex];
            }
        }
        if((autoOn&&(currentTargetIndex == 0&&!approachingTarget&&!mode1TimeUp))||allTargetsReached){
                //if autonomous, and on first target, and not not approaching target, and not mode 1 time up, or reached last target.
            mode =1;//wtf is mode
            distanceMultiplier = 50;//wthis is how heavily to rely on vision
        } else if((autoOn&&currentTargetIndex == 0&&mode1TimeUp)||(autoOn&&approachingTarget&&(currentTargetIndex<=OPEN_FIELD_INDEX||currentTargetIndex>=maxTargetIndex-END_LANE_INDEX))){
            mode =2;
            distanceMultiplier = 50;
        } else if((autoOn&&currentTargetIndex!=0)){
            mode =3;
            distanceMultiplier = 12;
        }
        flagPointDistance = D((currentXY.x-flagXY.x),(currentXY.y-flagXY.y));// basically the distance formula, but to what? what flags GPS point?
        if(allTargetsReached&&flagPointDistance<FLAG_DIST_THRESH){
            mode =4;// what is mode
        }
#ifdef FLAG_TESTING
        /*FLAG TESTING*/
        mode=4;
#endif //FLAG_TESTING

        /*Current Target Heading PID Control Adjustment*/
        cvar.lookAhead = 0.00;//?
        cvar.kP = 0.20; cvar.kI = 0.000; cvar.kD = 0.15;

        turn = cvar.turn;


        int bestVisGpsMask = 99;
        int h = 0;
        double minVisGpsTurn = 9999;
        for(h=0;h<11;h++){
            if(fabs((cvar.turn-turn_angle[h]))<minVisGpsTurn){
                minVisGpsTurn=fabs((cvar.turn-turn_angle[h]));
                bestVisGpsMask = h;
            }
        }
        bestGpsMask = bestVisGpsMask;
//        printf("bvg: %d \n", bestVisGpsMask);
//        printf("vgt: %f cv3: %f\n", minVisGpsTurn,cvar3.turn);

#ifdef USE_VISION
//        double visTurnBoost = 0.50;
        double visTurnBoost = 1.0;
        if(imageProc(mode) == -1) break;
        if(mode==1||mode==2){
            turn = turn_angle[bestmask];
            turn *= visTurnBoost;
        }else if(mode==3 && fabs(turn_angle[bestmask])>0.70){
            turn = turn_angle[bestmask];
            turn *= visTurnBoost;
        }
#endif //USE_VISION
#ifdef USE_LIDAR
        updateSick();
//        findObjects();
#endif //USE_LIDAR

#ifdef USE_COMBINED_BUFFER//??????????
#define WORSTTHRESH 10
#define BESTTHRESH 3
        if(mode==4){
#ifdef USE_NORTH
            turn = (0.5*turn_angle[bestBlueMask]+0.5*turn_angle[bestRedMask]);
#else
            turn = (0.65*turn_angle[bestBlueMask]+0.35*turn_angle[bestRedMask]);
#endif
            turn *= 0.75;
        }
        combinedTargDist = cvar.targdist;
        if(((approachingTarget||inLastTarget)&&currentTargetIndex>OPEN_FIELD_INDEX
            &&currentTargetIndex<maxTargetIndex-END_LANE_INDEX)||(MAG(howbad[worstmask]-howbad[bestmask]))<BESTTHRESH||mode==4){
            getCombinedBufferAngles(0,0);//Don't Use Vision Radar Data
        }else{
            getCombinedBufferAngles(0,1);//Use Vision Radar Data
        }
        if(combinedBufferAngles.left != 0 || combinedBufferAngles.right !=0){
            if(mode == 1 || mode==2 || mode==3 || mode==4){
//            if(mode == 1 || mode==2 || mode==3){
//            if(mode==2 || mode==3){
//            if(mode==3){
                if(fabs(combinedBufferAngles.right)==fabs(combinedBufferAngles.left)){
                    double revTurn;
                    double revDistLeft, revDistRight;
                    int revIdx;
                    if(fabs(turn)<0.10) dir = -1.0;
                    if(fabs(combinedBufferAngles.left)>1.25) dir = -1.0;
                    if(dir<0){
                        revIdx = 540-RAD2DEG(combinedBufferAngles.left)*4;
                        revIdx = MIN(revIdx,1080);
                        revIdx = MAX(revIdx,0);
                        revDistLeft = LMSdata[revIdx];

                        revIdx = 540-RAD2DEG(combinedBufferAngles.right)*4;
                        revIdx = MIN(revIdx,1080);
                        revIdx = MAX(revIdx,0);
                        revDistRight = LMSdata[revIdx];
                        if(revDistLeft>=revDistRight){
                            revTurn = combinedBufferAngles.left;
                        }else {
                            revTurn = combinedBufferAngles.right;
                        }
                        turn = revTurn;
                    }else{
                        turn = turn_angle[bestmask];
                    }
                } else if(fabs(combinedBufferAngles.right-turn)<fabs(combinedBufferAngles.left-turn)){
//                } else if(turn<=0){
                    turn = combinedBufferAngles.right;
                }else {
                    turn = combinedBufferAngles.left;
                }
            }
        }
#endif //USE_COMBINED_BUFFER
        if(dir<0||revFrameCount!=0){
            dir = -1.0;
            revFrameCount = (revFrameCount+1)%REVFRAMES;
        }
        //        turn *= dir;
        turn = SIGN(turn) * MIN(fabs(turn), 1.0);
        speed = 1.0/(1.0+1.0*fabs(turn))*dir;
        speed = SIGN(speed) * MIN(fabs(speed), 1.0);
        if(!autoOn){
            maxSpeed = 60;
            targetIndexMem = 0;
        }else if(dir<0){
            maxSpeed = 30;
        }else if(mode<=2||(mode==3 && fabs(turn_angle[bestmask])>0.25)){
            maxSpeed = 60 - 25*fabs(turn);
//            maxSpeed = 70 - 35*fabs(turn);
//            maxSpeed = 90 - 50*fabs(turn);
//            maxSpeed = 100 - 65*fabs(turn);
        }else if(mode==4){
            maxSpeed = 45-20*fabs(turn);
        }else{
            maxSpeed = 85 - 50*fabs(turn);
//            maxSpeed = 100 - 65*fabs(turn);
//            maxSpeed = 110 - 70*fabs(turn);
//            maxSpeed = 120 - 85*fabs(turn);
        }
        if(autoOn){
            lSpeed = (speed + turnBoost*turn) * maxSpeed;
            rSpeed = (speed - turnBoost*turn) * maxSpeed;
        }
#ifdef DEBUG_MAIN
        printf("s:%.4f t: %.4f m: %d vt:%f dir:%f tmr: %f\n", speed, turn, mode, turn_angle[bestmask], flagPointDistance, totalAutoTime);
#endif //DEBUG_MAIN
#ifdef DUMP_GPS
    if(dumpCount==0){
        if (fp != NULL) {
                fprintf(fp, "%f %f %f %f %f\n",gpsvar.latitude,gpsvar.longitude, gpsvar.course, gpsvar.speed, gpsvar.time);
            }
    }
        dumpCount = dumpCount+1%DUMPGPSDELAY;

#endif //DUMP_GPS
#ifdef DEBUG_TARGET
        debugTarget();
#endif //DEBUG_TARGET
#ifdef DEBUG_GUIDE
        debugGuide();
#endif //DEBUG_GUIDE
#ifdef DEBUG_GPS
        debugGPS();
#endif //DEBUG_GPS
#ifdef DEBUG_LIDAR
        debugSICK();
#endif //DEBUG_LIDAR
#ifdef DEBUG_BUFFER
        debugCombinedBufferAngles();
#endif //DEBUG_BUFFE
#ifdef DEBUG_VISUALIZER
        robotX = currentXY.x;
        robotY = currentXY.y;
        robotTheta = robotTheta;//redundant I know....
        targetX = targetXY.x;
        targetY = targetXY.y;
//        should probably pass the above to the function...
        paintPathPlanner(robotX,robotY,robotTheta);
        showPlot();
#endif //VISUALIZER

#ifdef USE_MAP
       if(mapCount==0){
//            mapRobot(currentXY.x,currentXY.y,robotTheta);
            if(clearMapCount==0) clearMapSection(currentXY.x,currentXY.y,robotTheta);
            else clearMapCount = (clearMapCount+1)%CLEARMAPDELAY;
            mapVSICK(currentXY.x,currentXY.y,robotTheta);
//            mapVSICK(0,0,0);
#ifdef USE_LIDAR
            mapSICK(currentXY.x,currentXY.y,robotTheta);
#endif
            showMap();
//            printf("MAPPING\n");
       }
            mapCount= (mapCount+1)%MAPDELAY;

#endif //USE_MAP
        sendSpeed(lSpeed,rSpeed);
        Sleep(5);
    }
#ifdef DUMP_GPS
    fclose(fp);
#endif
    return 0;
}
Exemplo n.º 6
0
void WorkThread::run()
{
    ztpmRecvDevInfo = new ZTPManager(8318,QHostAddress("224.102.228.40"));
    connect(ztpmRecvDevInfo,SIGNAL(readyRead()),this,SLOT(recvDevInfo()));

    sendTempTimerId = startTimer(2000);
//    QTimer* t = new QTimer;
//    t->setInterval(100);
//    t->start();
//    connect(t,SIGNAL(timeout()),this,SLOT(test()));
    qDebug("run");
    //GlobalInfo::getInstance()->debugStack.appendStack(QThread::currentThreadId(),"ready entry run");
    //QTimer::singleShot(3000,this,SLOT(test()));
    ztpm = new ZTPManager(3320,QHostAddress("224.102.228.40"));//塞拉门状态
    connect(ztpm,SIGNAL(readyRead()),this,SLOT(onRecvNetMsg()));
    ztpm = new ZTPManager;//专用来发送包

    ZTPManager* g_infoZtpm = new ZTPManager(8311,QHostAddress(BROADCAST_IP));
    connect(g_infoZtpm,SIGNAL(readyRead()),this,SLOT(onRecvNetMsg()));//G_INFO包中仅更新广播控制器串口数据缓存,可以放在子线程。
    modbusManager = new ModbusManager("/dev/ttyUSB1",4);
    modbusDog = new SoftWatchdog(modbusManager,modbusDeal);
    connect(modbusManager,SIGNAL(readyRead()),this,SLOT(recvModbus()),Qt::DirectConnection);
    bool res = modbusManager->open();
    if(res == true)
    {
        qDebug("open modbusManager success.");
    }
    else
        qDebug("open modbusManager failed.");

    mapTableSwitch_timer_id = startTimer(10000);
    if(mapTableSwitch_timer_id == 0)
        qDebug("Create mapTableSwitch_timer_id falied!!");
    else
        qDebug("Create mapTableSwitch_timer_id success!!");
//    QTimer timerMapTableSwitch;
//    timerMapTableSwitch.setInterval(10000);
//    timerMapTableSwitch.setSingleShot(false);
//    connect(&timerMapTableSwitch,SIGNAL(timeout()),this,SLOT(sendMapTableSwitch()),Qt::DirectConnection);
//    timerMapTableSwitch.start();
    gpsDevice = new GPSDevice("/dev/ttymxc1");
    g_rmc = &rmc;
    gpsSoftDog = new SoftWatchdog(gpsDevice,gpsDeal);
//    SamplesList v;
//    v.push_back(GeogPixSample(1991,51,106.805015,-20.675571));
//    v.push_back(GeogPixSample(776,2730,100.120199,-6.423076));
//    v.push_back(GeogPixSample(861,1414,100.578835,-13.555069));
//    v.push_back(GeogPixSample(1743,951,105.426127,-15.991936));
//    mapManager = new GPSMapManager("/appbin/GPSMap.png",v);
//    mapManager->setLat(-11.555069);
//    mapManager->setLng(100.426127);
    connect(gpsDevice,SIGNAL(readyRead()),this,SLOT(readGPS()));
//    gpsTimer = new QTimer();
//    gpsTimer->setSingleShot(false);
//    gpsTimer->setInterval(3000);
    //connect(gpsTimer,SIGNAL(timeout()),this,SLOT(gpsProccess()));
//    gpsTimer->start();

    comBroadcastManager = new BroadcastManager("/dev/ttymxc2");
    comBroadcastManager->setPwdFlag(true);
    connect(comBroadcastManager,SIGNAL(readyRead()),this,SLOT(comBroadcastProc()));
    comSoftDog = new SoftWatchdog(comBroadcastManager,comDeal);

    ztpBroadcastManager = new ZTPManager(BROADCAST_PORT,QHostAddress(BROADCAST_IP));
    connect(ztpBroadcastManager,SIGNAL(readyRead()),this,SLOT(onRecvNetMsg()));

    //定时发送系统广播
    QTimer timerBroadcast;
    timerBroadcast.setSingleShot(false);
    timerBroadcast.setInterval(1000);
    timerBroadcast.start();
    connect(&timerBroadcast,SIGNAL(timeout()),this,SLOT(onSendSysBroadcast()));
    new ZDownloadThread;
    exec();
    //GlobalInfo::getInstance()->debugStack.appendStack(QThread::currentThreadId(),"leave run");
}
Exemplo n.º 7
0
void setup()
{
	CardReaderValue.cardStatus = MM_UNDEFINED_ERROR;

	Serial.begin(115200);
	Serial.println("Init");

	//init Display
	displayStatus = TFTInit();
	switch (displayStatus)
	{
		case MM_SUCCESS:
			DisplayMaster("Display Initialisiert\n", DebugPC);
			Serial.println("Display Initialisiert");
			break;

		default:
			DisplayError("Display Init-ERROR!", DebugPC);
			Serial.println("Display Init-ERROR!");
			DebugPC = false;
			break;
	}


	//init GPS
	p_myGPSData = &myGPSData;
	initGPS();

	//Versuche ein GPS Signal zu bekommen, damit die Logdatei das Datum als Namen bekommt
	while(p_myGPSData->status != MM_SUCCESS)
	{
		DisplayMaster("GPS noch nicht Initialisiert\n", DebugPC);
		Serial.println("GPS noch nicht Initialisiert");
		delay(2500);
		readGPS(p_myGPSData);

	}


	//init CardReader
	char * FileName = "";
	String sFileName = "GPS_Log" + String(myGPSData.year, DEC) + "_" + String(myGPSData.month, DEC)+ "_" + String(myGPSData.day, DEC) + ".txt";
	sFileName.toCharArray(FileName, sFileName.length() + 1);

	CardReaderValue = SDInit(FileName);

	switch (CardReaderValue.cardStatus)
	{
		case MM_SUCCESS:
			DisplayMaster("CardReader Initialisiert\n", DebugPC);
			Serial.println("CardReader Initialisiert");
			break;

		default:
			DisplayError("CardReader Init-ERROR!", DebugPC);
			Serial.println("CardReader Init-ERROR!");
			break;
	}



}
Exemplo n.º 8
0
void geocacheMenu(void)
{
    int pressedKey = mySwitch.checkKeypad();
    switch (menuState)
    {
    case MENU0:
        tft.fillScreen(ST7735_BLACK);
        setCursorTFT(0,0);
        tft.print(menuHeader);
        setCursorTFT(1,0);
        tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
        tft.print(monGPSLocn);
        setCursorTFT(2,0);
        tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
        tft.print(monGPSClk);
        setCursorTFT(3,0);
        tft.print(downldWays);
        setCursorTFT(4,0);
        tft.print(goToWaypts);
        setCursorTFT(5,0);
        tft.print(setActWays);
        setCursorTFT(6,0);
        tft.print(showWaypts);
        menuState = MENU0B;
        break;
    case MENU0B:
        if (pressedKey != NOKEY)
        {
            if ((pressedKey == SELECT) || (pressedKey == RIGHT))
            {
                tft.fillScreen(ST7735_BLACK);
                setCursorTFT(0,0);
                tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
                tft.print(monGPSLocn);
                tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
                clearLine(1);
                clearLine(2);
                clearLine(3);
                clearLine(4);
                clearLine(5);
                clearLine(6);
                menuState = MENU0C;
            }
            else if (pressedKey == DOWN)
                menuState = MENU1;
        }
        break;
    case MENU0C:
        if (pressedKey != NOKEY)
            menuState = MENU0;
        readGPS();
        break;
    case MENU1:
        tft.fillScreen(ST7735_BLACK);
        setCursorTFT(0,0);
        tft.print(menuHeader);
        setCursorTFT(1,0);
        tft.print(monGPSLocn);
        setCursorTFT(2,0);
        tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
        tft.print(monGPSClk);
        tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
        setCursorTFT(3,0);
        tft.print(downldWays);
        setCursorTFT(4,0);
        tft.print(goToWaypts);
        setCursorTFT(5,0);
        tft.print(setActWays);
        setCursorTFT(6,0);
        tft.print(showWaypts);
        menuState = MENU1B;
        break;
    case MENU1B:
        if (pressedKey != NOKEY)
        {
            if ((pressedKey == SELECT) || (pressedKey == RIGHT))
            {
                clearLine(0);
                tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
                tft.print(monGPSClk);
                tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
                clearLine(1);
                clearLine(2);
                clearLine(3);
                clearLine(4);
                clearLine(5);
                clearLine(6);
                menuState = MENU1C;
            }
            else if (pressedKey == UP)
                menuState = MENU0;
            else if (pressedKey == DOWN)
                menuState = MENU2;
        }
        break;
    case MENU1C:
        if (pressedKey != NOKEY)
        {
            tft.fillScreen(ST7735_BLACK);
            menuState = MENU1;
        }
        GPSClock();
        break;
    case MENU2:
        tft.fillScreen(ST7735_BLACK);
        setCursorTFT(0,0);
        tft.print(menuHeader);
        setCursorTFT(1,0);
        tft.print(monGPSLocn);
        setCursorTFT(2,0);
        tft.print(monGPSClk);
        setCursorTFT(3,0);
        tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
        tft.print(downldWays);
        tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
        setCursorTFT(4,0);
        tft.print(goToWaypts);
        setCursorTFT(5,0);
        tft.print(setActWays);
        setCursorTFT(6,0);
        tft.print(showWaypts);
        menuState = MENU2B;
        break;
    case MENU2B:
    {
        int charIn;
        if (pressedKey != NOKEY)
        {
            if ((pressedKey == SELECT) || (pressedKey == RIGHT))
            {
                rxCount = 0;
                clearLine(2);
                clearLine(3);
                clearLine(4);
                clearLine(5);
                clearLine(6);
                clearLine(1);
                tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
                tft.print(downldWays);
                tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
                setCursorTFT(2,0);
                menuState = MENU2C;
                while (Serial.available())    // flush the serial read port
                    charIn = Serial.read();
                Serial.println("<DL>");    // signals host that it's time to load waypoints
            }
            else if (pressedKey == UP)
                menuState = MENU1;
            else if (pressedKey == DOWN)
                menuState = MENU3;
        }
    }
    break;
    case MENU2C:
    {
        int charIn;
        int errorCode;
        if (Serial.available())
        {
            charIn = Serial.read();
            if (charIn == '\n')
            {
                tft.print(".");
                rxBuffer[rxCount] = 0;
                errorCode = parseRxBuffer();
                if (errorCode == -1)
                {
                    clearLine(2);
                    tft.print("Error: Missing equal");
                }
                else if (errorCode == -2)
                {
                    clearLine(2);
                    tft.print("Error: Bad waypt num");
                }
                rxCount = 0;
                Serial.write('A');
                if ((rxBuffer[0] == '1') && (rxBuffer[1]=='9'))
                {
                    clearLine(3);
                    tft.print("Download Completed  ");
                    EEPROM_writeAnything(0, myStoreVals);
                    Serial.println("</SERIN>");    // signals host that it's time to load waypoints
                    delay(2000);  // 2 second message
                    menuState = MENU2;
                }
            }
            else
            {
                rxBuffer[rxCount++] = charIn;
            }
        }
        if (pressedKey == UP)
        {
            Serial.println("</DL>");    // signals host that it's time to load waypoints
            clearLine(2);
            tft.print("Download Terminated ");
            delay(2000);  // 2 second message
            menuState = MENU2;
        }
    }
    break;
    case MENU3:
        tft.fillScreen(ST7735_BLACK);
        setCursorTFT(0,0);
        tft.print(menuHeader);
        setCursorTFT(1,0);
        tft.print(monGPSLocn);
        setCursorTFT(2,0);
        tft.print(monGPSClk);
        setCursorTFT(3,0);
        tft.print(downldWays);
        setCursorTFT(4,0);
        tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
        tft.print(goToWaypts);
        tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
        setCursorTFT(5,0);
        tft.print(setActWays);
        setCursorTFT(6,0);
        tft.print(showWaypts);
        menuState = MENU3B;
        break;
    case MENU3B:
        if (pressedKey != NOKEY)
        {
            if ((pressedKey == SELECT) || (pressedKey == RIGHT))
            {
                quietReadGPS();
                delay(500);
                menuState = MENU3C;
            }
            else if (pressedKey == UP)
                menuState = MENU2;
            else if (pressedKey == DOWN)
                menuState = MENU4;
        }
        break;
    case MENU3C:
        tft.fillScreen(ST7735_BLACK);
        tft.drawCircle(64,110,45,ST7735_WHITE);
        setCursorTFT(0,0);
        tft.print(menuHeader);
        setCursorTFT(1,0);
        tft.print("Going to Waypoint   ");
        setCursorTFT(1,18);
        tft.print(currentWayPoint, DEC);
        setCursorTFT(2,0);
        tft.print("Bearing =");
        setCursorTFT(3,0);
        tft.print("Distance =");
        setCursorTFT(4,0);
        tft.print("Direction =");
        setCursorTFT(5,0);
        tft.print("Satellites =");
        quietReadGPS();
        setCursorTFT(2,10);
        tft.print("         ");
        setCursorTFT(2,10);
        bearing = calc_bearing(fLat2,fLon2,myStoreVals.lats[currentWayPoint],myStoreVals.lons[currentWayPoint]);
        tft.print(bearing);
        lastLat = fLat2;
        setCursorTFT(3,11);
        tft.print("        ");
        setCursorTFT(3,11);
        tft.print(calc_dist(fLat2,fLon2,myStoreVals.lats[currentWayPoint],myStoreVals.lons[currentWayPoint]));
        lastLon = fLon2;
        setCursorTFT(4,12);
        tft.print(GPS.angle-bearing);
        lastAngle = GPS.angle;
        lastBearing = bearing;

        setCursorTFT(5,13);
        tft.print(GPS.satellites);
        lastSats = GPS.satellites;
        menuState = MENU3D;
        break;
    case MENU3D:
    {
        if (pressedKey != NOKEY)
        {
            menuState = MENU3;
            break;
        }
        if (quietReadGPS() != 0)
        {
            break;
        }
        float distCalc = calc_dist(fLat2,fLon2,myStoreVals.lats[currentWayPoint],myStoreVals.lons[currentWayPoint]);
        bearing = calc_bearing(fLat2,fLon2,myStoreVals.lats[currentWayPoint],myStoreVals.lons[currentWayPoint]);
        if ((fLat2 != lastLat) || (fLon2 != lastLon))
        {
            setCursorTFT(2,10);
            tft.print("           ");
            setCursorTFT(2,10);
            tft.print(bearing);
            setCursorTFT(3,11);
            tft.print("          ");
            setCursorTFT(3,11);
            tft.print(distCalc);
            lastLat = fLat2;
            lastLon = fLon2;
        }
        if ((GPS.angle != lastAngle) || (bearing != lastBearing))
        {
            setCursorTFT(4,12);
            tft.print("         ");
            setCursorTFT(4,12);
            tft.print(GPS.angle-bearing);
            drawVector(GPS.angle-bearing);
            lastAngle = GPS.angle;
            lastBearing = bearing;
        }
        if (lastSats != GPS.satellites)
        {
            setCursorTFT(5,13);
            tft.print(GPS.satellites);
            tft.print("  ");
        }
    }
    break;
    case MENU4:
        tft.fillScreen(ST7735_BLACK);
        setCursorTFT(0,0);
        tft.print(menuHeader);
        setCursorTFT(1,0);
        tft.print(monGPSLocn);
        setCursorTFT(2,0);
        tft.print(monGPSClk);
        setCursorTFT(3,0);
        tft.print(downldWays);
        setCursorTFT(4,0);
        tft.print(goToWaypts);
        setCursorTFT(5,0);
        tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
        tft.print(setActWays);
        tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
        setCursorTFT(6,0);
        tft.print(showWaypts);
        menuState = MENU4B;
        break;
    case MENU4B:
        if (pressedKey != NOKEY)
        {
            if ((pressedKey == SELECT) || (pressedKey == RIGHT))
            {
                tft.fillScreen(ST7735_BLACK);
                setCursorTFT(0,0);
                tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
                tft.print(setActWays);
                tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
                setCursorTFT(1,0);
                tft.print(currentWayPoint, DEC);
                menuState = MENU4C;
            }
            else if (pressedKey == UP)
            {
                menuState = MENU3;
            }
            else if (pressedKey == DOWN)
            {
                menuState = MENU5;
            }
        }
        break;
    case MENU4C:
    {
        if (pressedKey != NOKEY)
        {
            switch(pressedKey)
            {
            case SELECT:
                myStoreVals.myCurrentWayPoint = currentWayPoint;
                EEPROM_writeAnything(0, myStoreVals);
                menuState = MENU4;
                break;
            case UP:
                if (currentWayPoint < 19)
                    currentWayPoint++;
                delay(250);
                break;
            case DOWN:
                if (currentWayPoint > 0)
                    currentWayPoint--;
                delay(250);
                break;
            }
            clearLine(1);
            tft.print(currentWayPoint, DEC);
        }
    }
    break;
    case MENU5:
        tft.fillScreen(ST7735_BLACK);
        setCursorTFT(0,0);
        tft.print(menuHeader);
        setCursorTFT(1,0);
        tft.print(monGPSLocn);
        setCursorTFT(2,0);
        tft.print(monGPSClk);
        setCursorTFT(3,0);
        tft.print(downldWays);
        setCursorTFT(4,0);
        tft.print(goToWaypts);
        setCursorTFT(5,0);
        tft.print(setActWays);
        setCursorTFT(6,0);
        tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
        tft.print(showWaypts);
        tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
        menuState = MENU5B;
        break;
    case MENU5B:
        if (pressedKey != NOKEY)
        {
            if ((pressedKey == SELECT) || (pressedKey == RIGHT))
            {
                tft.fillScreen(ST7735_BLACK);
                setCursorTFT(0,0);
                tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
                tft.print(showWaypts);
                tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
                for (int row = 0; row < 15; row++)
                {
                    setCursorTFT(row+1,0);
                    tft.print(row);
                    tft.print(",");
                    tft.print(myStoreVals.lats[row],4);
                    tft.print(",");
                    tft.print(myStoreVals.lons[row],4);
                }
                menuState = MENU5C;
            }
            else if (pressedKey == UP)
            {
                menuState = MENU4;
            }
        }
        break;
    case MENU5C:
    {
        if (pressedKey != NOKEY)
        {
            switch(pressedKey)
            {
            case SELECT:
            case UP:
                tft.fillScreen(ST7735_BLACK);
                menuState = MENU5;
                break;
            }
        }
        break;
    }
    }
}
Exemplo n.º 9
0
int main(){
  /*struct CharachterBuffer test;
  test.defaultBuffer(7);
  char pull;
  printf("empty pull (output riding input) test result: %d\n",test.pull(&pull));
  printf("empty push test result: %d\n",test.push('$'));
  printf("immenent output riding input occupied pull test result: %d\n",test.pull(&pull));
  printf("data integrity check: %c\n",pull);
  printf("output riding input push test result: %d\n",test.push('H'));
  printf("input && output riding nothing push test result: %d\n",test.push('I'));
  printf("input && output riding nothing push test result: %d\n",test.push(' '));
  printf("input && output riding nothing push test result: %d\n",test.push('M'));
  printf("input && output riding nothing push test result: %d\n",test.push('O'));
  printf("input && output riding nothing push test result: %d\n",test.push('M'));
  printf("immenent input riding output push test result: %d\n",test.push('!'));
  printf("input riding output push test result: %d\n",test.push('?'));
  
  
  int flow = 0;
  std::string gather = "";
  while((flow = test.pull(&pull)) != test.ERROR_COLLISION){
    printf("The input int is: %d\n",flow);
    gather += pull;
    if(flow == test.ERROR_COLLISION){
      printf("WHOA, DUDE!");
      break;
    }
  }
  printf("%s\n\n\n\n**********\n\n\n",gather.c_str());
  printf("bufferOutput: %d\n",test.bufferOutputPosition);
  printf("bufferInput: %d\n",test.bufferInputPosition);
  */
  gpsBuffer.defaultBuffer(1024);
  
  setup();
  
  /* //Tests Buffer and getNMEAstr()
  gpsBuffer.push(' ');
  gpsBuffer.push('$');
  gpsBuffer.push('T');
  gpsBuffer.push('E');
  gpsBuffer.push('S');
  gpsBuffer.push('T');
  gpsBuffer.push('\n');
  
  std::string gather="";
  getNMEAstr(&gather);
  printf("%s\n",gather.c_str());*/
  
  
  int overflow = readGPS();
  while( overflow != ERR_SHTAP){
    overflow = readGPS();
  }
  
  //printf(gpsBuffer.buffer);
  
  std::string gather="";

  getNMEAstr(&gather);
  struct nmea_data *gathered_data = new nmea_data(gather);
  printf("%s\n",gather.c_str());
  printA(gathered_data);



  gather = "";
  getNMEAstr(&gather);
  gathered_data = new nmea_data(gather);
  printf("%s\n",gather.c_str());
  printA(gathered_data);
  
  gather = "";
  getNMEAstr(&gather);
  gathered_data = new nmea_data(gather);
  printf("%s\n",gather.c_str());
  printA(gathered_data);
  
  gather = "";
  getNMEAstr(&gather);
  gathered_data = new nmea_data(gather);
  printf("%s\n",gather.c_str());
  printA(gathered_data);
  
  gather = "";
  getNMEAstr(&gather);
  gathered_data = new nmea_data(gather);
  printf("%s\n",gather.c_str());
  printA(gathered_data);
  
  gather = "";
  getNMEAstr(&gather);
  gathered_data = new nmea_data(gather);
  printf("%s\n",gather.c_str());
  printA(gathered_data);
  
  
  gather = "";
  
  /*setup();
  
  char input;
  while(1){
    read(serialHandle, &input, 1);
    if(input != 0){
      printf("%c\n",input);
    }
  }*/
  
  
}