bool GPSSensorNMEA::GetImpact(GPSSensorNMEA::gpsImpact *pdata) {
if(fixed) {
memcpy((char*)pdata,(const char*)&impact,sizeof(gpsImpact));
DBG_MEMDUMP("data",(char*)pdata,sizeof(gpsImpact));
return true;
}
return false;
}
void GPSSensorNMEA::Sample(void) {
int len = 0;
bool newData = false;
if(gpsNewlineDetected) {
while(1) {
int nChars = gps.rxBufferGetCount();
if(nChars == 0)
break;
//else
// DBG("To read --> %d",nChars);
// We could consider passing the data directly to parser whithout buffering it
// but: That's annoying cause if so we could not printf them smoothly on console
len = GetLine();
//DBG("GotLine(%d) %s",len,recvBuff);
DBG_MEMDUMP("GPSData",(const char*)recvBuff,len);
for(unsigned int i = 0; i < len; i++) {
// Insert all data to gpsParser
if(gpsParser.encode(recvBuff[i])) {
newData = true;
}
}
// Only take a sample when all the data for each epoch have been received
if(gpsParser.gll_ready()){
long lat = 0, lon = 0, hdop = 0;
unsigned long date = 0, time = 0;
long alt = 0;
gpsParser.get_position(&lat,&lon,&alt);
gpsParser.get_datetime(&date,&time);
hdop = gpsParser.hdop();
if((lat != TinyGPS::GPS_INVALID_ANGLE) && (lon != TinyGPS::GPS_INVALID_ANGLE)){
fixed = true;
impact.date = date;
impact.time = time/100;
impact.lat = lat;
impact.lon = lon;
impact.alt = alt;
if(hdop>0xffff) {
impact.hdop = -1;
} else {
impact.hdop = hdop;
}
WARN("######[%ld-%ld][%08x - %09ld|%08x - %09ld|%04d]######",impact.date,impact.time,impact.lat,impact.lat,impact.lon,impact.lon,impact.hdop);
} else {
fixed = false;
DBG("All data have been received but lat/lon does not seems to be valid",date,time,lat,lon,hdop);
}
gpsParser.reset_ready();
} else {
fixed = false;
}
}
}
}
int GPSSensorUBX::ReceiveFrame(uint32_t timeOutUs){
bool reading = false;
int i = 0;
int j = 0;
int jMax = 10;
uint32_t deadLine = us_ticker_read() + timeOutUs;
while(1) {
//DBG("PIPI");
if(gps.readable()) {
j = 0;
//DBG("Rx");
recvBuff[i++] = gps.getc();
reading = true;
} else {
// DBG("CACA");
// Was reading and become unreadable
if(reading) {
if(j<jMax) {
j++;
Thread::wait(4);
} else {
DBG_MEMDUMP("Received", (const char*) recvBuff, i);
return i;
}
} else {
if(us_ticker_read()>deadLine) {
DBG("TimedOut");
return 0;
} else {
Thread::wait(10);
//DBG("ZIZI");
}
}
}
}
}
static int
main_testtransfer(int argc, char **argv)
{
/* CBM_FILE fd; */
unsigned char compare_buffer[0x8000];
unsigned char buffer[0x8000];
unsigned int error = 0;
int rv;
#if DBG
DbgFlags = 0x0;
#endif
printf(" libtransfer sample: " __DATE__ " " __TIME__ "\n");
if (argc > 1)
{
processParameter(argc, argv);
}
rv = cbm_driver_open(&fd, 0);
if(rv != 0)
{
DBG_ERROR((DBG_PREFIX "cbm_driver_open() failed!"));
return 1;
}
DBG_PRINT((DBG_PREFIX "before install"));
libopencbmtransfer_install(fd, drive);
memset(buffer, 0, sizeof(buffer));
if (compare)
read_from_file(compare_buffer, sizeof(compare_buffer), "rom.image");
DBG_PRINT((DBG_PREFIX "before read_mem"));
#if 0
transferlength = 0x100;
printf("write $0300\n");
libopencbmtransfer_write_mem(fd, drive, compare_buffer, 0x300, transferlength);
printf("read $0300\n");
libopencbmtransfer_read_mem(fd, drive, buffer, 0x300, transferlength);
printf("compare\n");
if (memcmp(buffer, compare_buffer, transferlength) != 0)
{
printf("differing!\n");
}
#else
while (count--)
{
printf("read: %i, error = %u: \n", count+1, error);
libopencbmtransfer_read_mem(fd, drive, buffer, 0x8000, transferlength);
if (compare)
{
if (memcmp(buffer, compare_buffer, transferlength) != 0)
{
char filename[128];
int n;
printf("\n\n***** ERROR COMPARING DATA! *****\n\n");
++error;
strcpy(filename, "image.err.");
n = strlen(filename);
if (error > 9999) filename[n++] = ((error / 10000) % 10) + '0';
if (error > 999) filename[n++] = ((error / 1000) % 10) + '0';
if (error > 99) filename[n++] = ((error / 100) % 10) + '0';
if (error > 9) filename[n++] = ((error / 10) % 10) + '0';
if (error > 0) filename[n++] = ((error / 1) % 10) + '0';
filename[n] = 0;
write_to_file(buffer, transferlength, filename);
}
else
{
printf(" compare success\n");
}
}
printf("write: %i, error = %u: \n", count+1, error);
libopencbmtransfer_write_mem(fd, drive, compare_buffer, 0x8000, transferlength);
}
#endif
if (outputdump)
DBG_MEMDUMP("8000:", buffer, transferlength + 0x10);
if (writedumpfile)
write_to_file(buffer, transferlength, "image.bin");
DBG_PRINT((DBG_PREFIX "before remove"));
libopencbmtransfer_remove(fd, drive);
cbm_driver_close(fd);
FUNC_LEAVE_INT(0);
}
