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);
}
}
}
