bool navcoreBDParse ::processBDGSV(const char *sentence) { UINT32 count; UINT8 i; UINT8 index; INT32 message; INT32 messages; INT32 value; char work[FIELD_BUFFER_LENGTH]; const char *field; lastBDType = NMEA_MESSAGE_TYPE_GSV; field = sentence; message = GPS_BADVALUE; messages = GPS_BADVALUE; // messages if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { messages = decodeIntegerValue(work); } } // message # if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { message = decodeIntegerValue(work); if (message == 1) { //set the flag indicating if the full BDGSV set of sentences have been received gps_fullBDGSV=FALSE; if (message==1){ gps_Rx_BDGSV1=TRUE; gps_Rx_BDGSV2=FALSE; gps_Rx_BDGSV3=FALSE; partialBDGSV.satelliteTracked = 0; } } } //@MIC gps_Rx_BDGSV4=FALSE; gps_Rx_BDGSV5=FALSE; gps_Rx_BDGSV6=FALSE; //@MIC if (messages==1){ gps_fullBDGSV=TRUE; } } if ((message==2) && (gps_Rx_BDGSV1==TRUE)){ gps_Rx_BDGSV2=TRUE; gps_Rx_BDGSV3=FALSE; //@MIC gps_Rx_BDGSV4=FALSE; gps_Rx_BDGSV5=FALSE; gps_Rx_BDGSV6=FALSE; //@MIC if (messages==2){ gps_fullBDGSV=TRUE; } } if ((message==3) && (gps_Rx_BDGSV2==TRUE)){ gps_Rx_BDGSV3=TRUE; //@MIC gps_Rx_BDGSV4=FALSE; gps_Rx_BDGSV5=FALSE; gps_Rx_BDGSV6=FALSE; //@MIC if (messages==3){ gps_fullBDGSV=TRUE; } } //@MIC if ((message==4) && (gps_Rx_BDGSV3==TRUE)){ gps_Rx_BDGSV4=TRUE; gps_Rx_BDGSV5=FALSE; gps_Rx_BDGSV6=FALSE; if (messages==4){ gps_fullBDGSV=TRUE; } } if ((message==5) && (gps_Rx_BDGSV4==TRUE)){ gps_Rx_BDGSV5=TRUE; gps_Rx_BDGSV6=FALSE; if (messages==5){ gps_fullBDGSV=TRUE; } } if ((message==6) && (gps_Rx_BDGSV5==TRUE)){ gps_Rx_BDGSV6=TRUE; if (messages==6){ gps_fullBDGSV=TRUE; } } //@MIC // iBDore messages with null message or message count fields if ( (messages == GPS_BADVALUE) || (message == GPS_BADVALUE) ) { return FALSE; } // check that message number is in range if ( (message < 1) || (message > 6/*3*/) ) { return FALSE; } // check that number of messages is in range if ( (messages < 1) || (messages > 6/*3*/) ) { return FALSE; } // for message number 1 initialise gsv rx parameters if ( message == 1 ) { gsvFieldCount = 0; gsvMessageCount = 0; count = partialBDGSV.sentence; initBDGSV(); partialBDGSV.sentence = count; } // increment message count gsvMessageCount++; // # satellites being tracked if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { if ( (value = decodeIntegerValue(work)) != GPS_BADVALUE ) { partialBDGSV.satelliteCount = (UINT8)value; gsvFieldCount++; } } } for ( i = 0; i < 4; i++ ) { index = (UINT8)(((message - 1) << 2) + i); // PRN number if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { if ( (value = decodeIntegerValue(work)) != GPS_BADVALUE ) { partialBDGSV.prn[index] = (UINT8)value; gsvFieldCount++; } } } // elevation if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { if ( (value = decodeIntegerValue(work)) != GPS_BADVALUE ) { partialBDGSV.elevation[index] = (UINT8)value; gsvFieldCount++; } } } // azimuth if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { if ( (value = decodeIntegerValue(work)) != GPS_BADVALUE ) { partialBDGSV.azimuth[index] = (UINT16)value; gsvFieldCount++; } } } // snr if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { if ( (value = decodeIntegerValue(work)) != GPS_BADVALUE ) { partialBDGSV.snr[index] = (UINT8)value; if ( value > 0 ) { ++partialBDGSV.satelliteTracked; } gsvFieldCount++; } else { partialBDGSV.snr[index] = 0; } } } } /* A complete sentence has all the appropriate fields filled */ /* A complete SET of sentences is also true if: */ /* if ( (message == messages) && (gsvMessageCount == messages) ) */ if ( gsvFieldCount >= messages ) { // perhaps copy sentences across to valid sentences when they become valid partialBDGSV.sentence++; memcpy(&gps_BDGSV,&partialBDGSV,sizeof(partialBDGSV)); return TRUE; } return FALSE; }
bool navcoreBDParse::processBDRMC(const char *sentence) { UINT32 count; UINT8 fieldCount; char work[FIELD_BUFFER_LENGTH]; const char *field; double outLat = 0; double outLon = 0; lastBDType = NMEA_MESSAGE_TYPE_RMC; field = sentence; fieldCount = 0; // initialise BDRMC sentence values count = partialBDRMC.sentence; initBDRMC(); partialBDRMC.sentence = count; successParse = FALSE; effectiveGPS = FALSE; // UTC if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 6 ) { if ( (partialBDRMC.utc = decodeTimeValue(work) ) != TIME_BAD_VALUE ) { fieldCount++; } } } // status if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { partialBDRMC.status = work[0]; if ( work[0] == 'A' ) { fieldCount++; //@TPE The counter is recorded when GPS status is fixed. if(FromUnfixToFx < FILTER_GPS_TIMES_FORM_UNFIX_TO_FIX) FromUnfixToFx++; effectiveGPS = TRUE; } else //@TPE The counter is reset when GPS status is unfixed. FromUnfixToFx = 0; } } // latitude if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 7 ) { if ( (partialBDRMC.latitude = decodeLatLong(work)) != GPS_BADVALUE ) { fieldCount++; } } } // latitude direction (N,S) if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { fieldCount++; if ( (*field == 'S') && (partialBDRMC.latitude != GPS_BADVALUE) ) { partialBDRMC.latitude = -partialBDRMC.latitude; } } } // longitude if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 7 ) { if ( (partialBDRMC.longitude = decodeLatLong(work)) != GPS_BADVALUE ) { fieldCount++; } } } // longitude direction (E,W) if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { fieldCount++; if ( (*field == 'W') && (partialBDRMC.longitude != GPS_BADVALUE) ) { partialBDRMC.longitude = -partialBDRMC.longitude; } } } // ground speed if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 3 ) { if ( (partialBDRMC.speed = decodeFloatValue(work)) != GPS_BADVALUE ) { fieldCount++; } } } // heading if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 3 ) { if ( (partialBDRMC.heading = decodeFloatValue(work)) != GPS_BADVALUE ) { fieldCount++; } } } // date if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 6 ) { if ( (partialBDRMC.date = time_stringToUlong(NULL,work)) != TIME_BAD_VALUE ) { fieldCount++; } } } if(partialBDRMC.utc>0&&partialBDRMC.latitude!=GPS_BADVALUE&&partialBDRMC.longitude!=GPS_BADVALUE) { successParse = TRUE; utc = partialBDRMC.utc; latitude = partialBDRMC.latitude; longitude = partialBDRMC.longitude; speed = partialBDRMC.speed; heading = partialBDRMC.heading; date = partialBDRMC.date; } else { utc = 0; latitude = GPS_BADVALUE; longitude = GPS_BADVALUE; } if ( fieldCount >= 1 ) { partialBDRMC.sentence++; //@TPE S /*LOG_INFO(0, ("processBDRMC (before CN-condition +[%d, %d]", partialBDRMC.latitude, partialBDRMC.longitude));*/ #ifdef _WIN32 if (strncmp(gloadMapName,"CN",2)==0){ /* LOG_INFO(0, ("processBDRMC+[%d, %d]", partialBDRMC.latitude, partialBDRMC.longitude));*/ ConverCHNCoords( partialBDRMC.latitude/100000.0, partialBDRMC.longitude/100000.0, &outLat, &outLon); partialBDRMC.latitude = (outLat*100000.0); partialBDRMC.longitude = (outLon*100000.0); /* LOG_INFO(0, ("processBDRMC-[%d, %d]", partialBDRMC.latitude, partialBDRMC.longitude));*/ } #endif //@TPE E memcpy(&gps_BDRMC,&partialBDRMC,sizeof(partialBDRMC)); return TRUE; } return FALSE; }
bool navcoreBDParse ::processBDGSA(const char *sentence) { UINT32 count; UINT8 i; UINT8 fieldCount; char work[FIELD_BUFFER_LENGTH]; const char *field; lastBDType = NMEA_MESSAGE_TYPE_GSA; field = sentence; fieldCount = 0; // initialise the BDGSA sentence parameters to defaults count = partialBDGSA.sentence; initBDGSA(); partialBDGSA.sentence = count; // mode if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { partialBDGSA.mode = work[0]; fieldCount++; } } // fix type if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { partialBDGSA.fixType = (UINT8)(work[0] - '0'); if ( (work[0] == '2') || (work[0] == '3') ) { fieldCount++; } } } // sv ID's for ( i = 0; i < 12; i++ ) { partialBDGSA.satelliteIDs[i] = 0; if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { // must be at least 3 valid fields for a fix partialBDGSA.satellitesUsed++; partialBDGSA.satelliteIDs[i] = (UINT8)atoi(work); if(atoi(work)>0 ) { BD2StatelliteNum++; } fieldCount++; } else { partialBDGSA.satelliteIDs[i] = 0; } } } // pdop if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 3 ) { if ( (partialBDGSA.pdop = decodeFloatValue(work)) != GPS_BADVALUE ) { fieldCount++; } } } // hdop if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 3 ) { if ( (partialBDGSA.hdop = decodeFloatValue(work)) != GPS_BADVALUE ) { fieldCount++; } } } // vdop if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 3 ) { if ( (partialBDGSA.vdop = decodeFloatValue(work)) != GPS_BADVALUE ) { fieldCount++; } } } if ( fieldCount >= 1 ) { partialBDGSA.sentence++; memcpy(&gps_BDGSA,&partialBDGSA,sizeof(partialBDGSA)); return TRUE; } return FALSE; }
bool navcoreBDParse ::processBDGGA(const char *sentence) { UINT32 count; UINT8 fieldCount; char work[FIELD_BUFFER_LENGTH]; const char *field; double outLat = 0; double outLon = 0; lastBDType = NMEA_MESSAGE_TYPE_GGA; field = sentence; fieldCount = 0; // initialise BDGGA message values to default count = partialBDGGA.sentence; initBDGGA(); partialBDGGA.sentence = count; successParse = FALSE; // UTC if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 6 ) { if ( (partialBDGGA.utc = decodeTimeValue(work) ) != TIME_BAD_VALUE ) { fieldCount++; } } } // latitude if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 7 ) { if ( (partialBDGGA.latitude = decodeLatLong(work)) != GPS_BADVALUE ) { fieldCount++; } } } // latitude direction (N,S) if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { fieldCount++; if ( (*field == 'S') && (partialBDGGA.latitude != GPS_BADVALUE) ) { partialBDGGA.latitude = -partialBDGGA.latitude; } } } // longitude if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 7 ) { if ( (partialBDGGA.longitude = decodeLatLong(work)) != GPS_BADVALUE ) { fieldCount++; } } } // longitude direction (E,W) if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { fieldCount++; if ( (*field == 'W') && (partialBDGGA.longitude != GPS_BADVALUE) ) { partialBDGGA.longitude = -partialBDGGA.longitude; } } } // quality if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { partialBDGGA.quality = work[0]; if ( work[0] != '0' ) { fieldCount++; } } } // SV's used if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { partialBDGGA.satellitesUsed = (UINT8)atoi(work); fieldCount++; } } // Horizontal Dilution (Ignore) if ( (field = findNextField(field)) != NULL ) { readField(work,field); } // altitude if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 3 ) { if ( (partialBDGGA.altitude = decodeFloatValue(work)) != GPS_BADVALUE ) { fieldCount++; } } } // altitude direction (+,-) if ( (field = findNextField(field)) != NULL ) { if ( readField(work,field) >= 1 ) { fieldCount++; if ( (*field == '-') && (partialBDGGA.altitude != GPS_BADVALUE) ) { partialBDGGA.altitude = -partialBDGGA.altitude; } } } if(partialBDGGA.utc>0&&partialBDGGA.latitude!=GPS_BADVALUE&&partialBDGGA.longitude!=GPS_BADVALUE) { successParse = TRUE; utc = partialBDGGA.utc; latitude = partialBDGGA.latitude; longitude=partialBDGGA.longitude; } else { utc = 0; latitude = GPS_BADVALUE; longitude = GPS_BADVALUE; } if ( fieldCount >= 1 ) { partialBDGGA.sentence++; memcpy(&gps_BDGGA,&partialBDGGA,sizeof(partialBDGGA)); return TRUE; } return FALSE; }
// parses the PTTKT sentence into the BD_partialPTTKT structure which is // copied over to the externally visible PTTKT structure if any non null // fields are decoded. BOOL8 navcoreBDParse ::processPTTKT(const char *sentence) { char work[FIELD_BUFFER_LENGTH]; UINT8 index; const char *field, *wrk; lastBDType = NMEA_MESSAGE_TYPE_TRAFFIC; field = sentence; // MESSAGE SUBTYPE if ( (field = findNextField(field)) != NULL ) { readField(work,field); if (strncmp (work,"DATA",4)==0) { // contains one field, which is 16 hex characters if ((field = findNextField(field)) != NULL) { if (readField(work,field) == 16) { // convert 16 hex characters into UINT8 buffer wrk = work; for (index = 0; index < 8; index++) { BD_partialPTTKT.tmcData[index] = hexToBinary (wrk); wrk += 2; } BD_partialPTTKT.lastMessage = TMC_MSG_TYPE_DATA; memcpy (&BD__PTTKT,&BD_partialPTTKT,sizeof(BD__PTTKT)); return TRUE; } } return FALSE; } else if (strncmp (work,"STAT",4)==0) { // contains 5 fields for (index = 0; index < 5; index++) { if ((field = findNextField(field)) != NULL) { readField(work,field); switch (index) { case 0: BD_partialPTTKT.hardwarePresent = (UINT8)atoi(work); break; case 1: // tuned frequency (decimal number) BD_partialPTTKT.frequency = (UINT16)atoi(work); break; case 2: // Block Count (decimal number) BD_partialPTTKT.blockCount = (UINT8)atoi(work); break; case 3: // Traffic Message Count BD_partialPTTKT.tmcCount = (UINT8)atoi(work); break; case 4: // error rate (decimal number) BD_partialPTTKT.errorRate = (UINT8)atoi(work); break; } } else { return FALSE; } } BD_partialPTTKT.lastMessage = TMC_MSG_TYPE_STATUS; memcpy (&BD__PTTKT,&BD_partialPTTKT,sizeof(BD__PTTKT)); return TRUE; } else if (strncmp (work,"TUNE",4)==0) { // returns the TUNE command sent to the hardware // need to get the tune status from this // contains 2 fields (only the last is of interest) for (index = 0; index < 2; index++) { if ((field = findNextField(field)) != NULL) { readField(work,field); switch (index) { case 0: // frequency break; case 1: // status (0 or 1) BD_partialPTTKT.tuneStatus = (UINT8)atoi(work); break; } } else { return FALSE; } } BD_partialPTTKT.lastMessage = TMC_MSG_TYPE_TUNE; memcpy (&BD__PTTKT,&BD_partialPTTKT,sizeof(BD__PTTKT)); return TRUE; } else if (strncmp (work,"WAKE",4)==0) { // The WAKE command is generated when // the (Wince) target comes out of sleep mode BD_partialPTTKT.lastMessage = TMC_MSG_TYPE_WAKEUP; BD__PTTKT.lastMessage = TMC_MSG_TYPE_WAKEUP; return TRUE; } else if (strncmp (work,"LIST",4)==0) { // contains 8 fields for (index = 0; index < 8; index++) { if ((field = findNextField(field)) != NULL) { readField(work,field); switch (index) { case 0: // Flag to indicate tuned freq BD_partialPTTKT.stnCurrent = (BOOL8)atoi(work); break; case 1: // Station Count (decimal number) BD_partialPTTKT.stnCount = (UINT8)atoi(work); break; case 2: // station Index (decimal number) BD_partialPTTKT.stnIndex = (UINT8)atoi(work); break; case 3: // station Frequency (decimal number) BD_partialPTTKT.stnFrequency = (UINT16)atoi(work); break; case 4: // station PI code (hex) wrk = work; BD_partialPTTKT.stnPI = (hexToBinary (wrk) << 8); wrk+=2; BD_partialPTTKT.stnPI += (hexToBinary (wrk)); break; case 5: // station signal strength BD_partialPTTKT.stnSignal = (UINT8)atoi(work); break; case 6: // station flag BD_partialPTTKT.stnFlag = (UINT8)atoi(work); break; case 7: // station name strcpy (BD_partialPTTKT.stnName, work); break; } } else { return FALSE; } } BD_partialPTTKT.lastMessage = TMC_MSG_TYPE_LIST; memcpy (&BD__PTTKT,&BD_partialPTTKT,sizeof(BD__PTTKT)); return TRUE; } else if (strncmp (work,"SGNL",4)==0) { // contains 3 fields freq, PI, signal // station list entries used for storage for (index = 0; index < 3; index++) { if ((field = findNextField(field)) != NULL) { readField(work,field); switch (index) { case 0: // station Frequency (decimal number) BD_partialPTTKT.stnFrequency = (UINT16)atoi(work); break; case 1: // station PI code (hex) wrk = work; BD_partialPTTKT.stnPI = (hexToBinary (wrk) << 8); wrk+=2; BD_partialPTTKT.stnPI += (hexToBinary (wrk)); break; case 2: // station signal strength BD_partialPTTKT.stnSignal = (UINT8)atoi(work); break; } } else { return FALSE; } } BD_partialPTTKT.lastMessage = TMC_MSG_TYPE_SIGSTRENGTH; memcpy (&BD__PTTKT,&BD_partialPTTKT,sizeof(BD__PTTKT)); return TRUE; } else if (strncmp (work,"RESET",5)==0) { // return the RESET command sent to the hardware BD_partialPTTKT.lastMessage = TMC_MSG_TYPE_RESET; BD__PTTKT.lastMessage = TMC_MSG_TYPE_RESET; return TRUE; } } return FALSE; }
void solve(Field &in) { int toSolve = FIELD_SIZE * FIELD_SIZE; int prefilled = 0; for(size_t i = 0; i < FIELD_SIZE; i++) { fill(rows[i].begin(), rows[i].end(), false); fill(cols[i].begin(), cols[i].end(), false); fill(boxes[i].begin(), boxes[i].end(), false); } F_LOOP { cost[i][j] = FIELD_SIZE * 3; } F_LOOP { auto val = in[i][j]; if(val != 0) { in[i][j] = 0; set(in, i, j, val); prefilled++; toSolve--; } } stack<Coord> visited; Coord next = findNextField(in); while(toSolve > 0) { //cout << "############" << endl; //cout << "Next: " << next.x << ", " << next.y << endl; //cout << "Depth: " << visited.size() << endl; //cout << "To-Solve: " << toSolve << endl; //print(in, next.x, next.y); assert(next.x != -1); char val = 0; auto startVal = in[next.x][next.y]; if(startVal != 0) { unset(in, next.x, next.y); toSolve++; } // Loop invariant assert(toSolve + visited.size() + prefilled == FIELD_SIZE * FIELD_SIZE); for(int i = startVal; i < FIELD_SIZE; i++) { //cout << "Try " << i + 1 << endl; if(!rows[next.x][i] && !cols[next.y][i] && !boxes[boxIdx(next.x, next.y)][i]) { val = i + 1; break; } } if(val == 0) { //cout << "Found no fit. " << endl; if(visited.size() == 0) { cout << "Unable to solve." << endl; } next = visited.top(); visited.pop(); } else { //cout << "Found " << val << endl; set(in, next.x, next.y, val); toSolve--; visited.push(next); next = findNextField(in); } } }