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);
}
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;
}
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();
}
}
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;
}
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&¤tTargetIndex == 0&&mode1TimeUp)||(autoOn&&approachingTarget&&(currentTargetIndex<=OPEN_FIELD_INDEX||currentTargetIndex>=maxTargetIndex-END_LANE_INDEX))){
mode =2;
distanceMultiplier = 50;
} else if((autoOn&¤tTargetIndex!=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)&¤tTargetIndex>OPEN_FIELD_INDEX
&¤tTargetIndex<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;
}
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");
}
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;
}
}
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;
}
}
}
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);
}
}*/
}