void
AP_GPS_UBLOX::_parse_gps(void)
{
switch (_msg_id) {
case MSG_POSLLH:
time = _buffer.posllh.time;
longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude;
altitude = _buffer.posllh.altitude_msl / 10;
break;
case MSG_STATUS:
fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
break;
case MSG_SOL:
fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
num_sats = _buffer.solution.satellites;
break;
case MSG_VELNED:
speed_3d = _buffer.velned.speed_3d; // cm/s
ground_speed = _buffer.velned.speed_2d; // cm/s
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
break;
}
_setTime();
valid_read = 1;
new_data = 1;
}
void DateTime::_setTime(const char* str)
{
if ( NULL == str)
{
return;
}
time_t tt;
struct tm tm_;
#if defined(_WINDOWS) || defined(_WIN32)
uint year, mon, day, hour, min, sec;
sscanf(str,"%d-%d-%d %d:%d:%d", &year, &mon, &day, &hour, &min, &sec);
tm_.tm_year = year - 1900;
tm_.tm_mon = mon - 1;
tm_.tm_mday = day;
tm_.tm_hour = hour;
tm_.tm_min = min;
tm_.tm_sec = sec;
tm_.tm_isdst = 0;
tt = mktime(&tm_);
#else
strptime(buf, "%Y-%m-%d %H:%M:%S", &tm_);
tm_.tm_isdst = 0;
tt = mktime(&tm_);
#endif
_setTime(tt);
}
void
AP_GPS_I2C::_parse_gps(void)
{
fix = (_buffer.i2cmsg.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.i2cmsg.fix_type == FIX_3D);
ground_speed = _buffer.i2cmsg.speed_2d; // cm/s
ground_course = _buffer.i2cmsg.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
_setTime();
valid_read = 1;
new_data = 1;
}
void LedTimer::setTimeAct( unsigned short minutes, unsigned short seconds )
{
stop();
if( minutes < 59
&& seconds < 59
&& 60 * minutes + seconds <= mSecondsMax )
mSecondsAct = 60 * minutes + seconds;
else
mSecondsAct = 0;
_setTime();
}
// Private Methods
void
AP_GPS_MTK::_parse_gps(void)
{
fix = (_buffer.msg.fix_type == FIX_3D);
latitude = _swapl(&_buffer.msg.latitude) * 10;
longitude = _swapl(&_buffer.msg.longitude) * 10;
altitude = _swapl(&_buffer.msg.altitude);
ground_speed = _swapl(&_buffer.msg.ground_speed);
ground_course = _swapl(&_buffer.msg.ground_course) / 10000;
num_sats = _buffer.msg.satellites;
// XXX docs say this is UTC, but our clients expect msToW
time = _swapl(&_buffer.msg.utc_time);
_setTime();
valid_read = true;
new_data = true;
}
bool LedTimer::update()
{
if( isRunning())
{
unsigned short timer = (int)ci::app::getElapsedSeconds();
mSecondsAct += math<unsigned short>::min( timer - mTimer, mSecondsMax - mSecondsAct );
mTimer = timer;
_setTime();
if( mSecondsAct == mSecondsMax )
{
stop();
mListener->callCallback();
}
return true;
}
return false;
}
// ------------------ calcBestVariant ----------------------
void CBriefcase::calcBestVariant()
{
_setTime();
for (int i=1; i<=ITER_COUNT; i++)
{
NoUsedProfit = 0;
BestCost = 0;
BestProfit = 0;
CurCost = 0;
CurProfit = 0;
for (int i=0; i<(int)NumItem; i++)
{
NoUsedProfit += invest[i].profit;
cur[i] = false;
best[i] = false;
}
search();
}
_getTime();
}
DateTime::DateTime(const char* str)
{
_setTime(str);
}
DateTime::DateTime(const uint64 millis)
{
_setTime(millis);
}
DateTime::DateTime()
{
time_t tt = time(NULL);
_setTime((uint64)tt);
}
/*
Set the new categories
0 fails
1 successes
*/
int _setNewCategories(LPWIN32LOCALE _locale, SHORT Category)
{
int Error = 1;
if (_locale->setLocaleReturn)
free(_locale->setLocaleReturn);
_locale->setLocaleReturn = NULL;
// Allocate the structures for the locale
if ((Category & LC_MONETARY) == LC_MONETARY)
{
if ((_locale->monetary = (LPWIN32MONETARY) malloc(sizeof(WIN32MONETARY))) == NULL)
{
SetLastError(ERROR_OUTOFMEMORY);
return NULL;
}
Error *= _setMonetary(_locale->handle, _locale->monetary);
if (Error != 0)
Error *= _addCategoryName(_locale, "LC_MONETARY");
}
if ((Category & LC_TIME) == LC_TIME)
{
if ((_locale->time = (LPWIN32TIME) malloc(sizeof(WIN32TIME))) == NULL)
{
SetLastError(ERROR_OUTOFMEMORY);
return NULL;
}
Error *= _setTime(_locale->handle, _locale->time);
if (Error != 0)
Error *= _addCategoryName(_locale, "LC_TIME");
}
if ((Category & LC_NUMERIC) == LC_NUMERIC)
{
if ((_locale->numeric = (LPWIN32NUMERIC) malloc(sizeof(WIN32NUMERIC))) == NULL)
{
SetLastError(ERROR_OUTOFMEMORY);
return NULL;
}
Error *= _setNumeric(_locale->handle, _locale->numeric);
if (Error != 0)
Error *= _addCategoryName(_locale, "LC_NUMERIC");
}
if ((Category & LC_COLLATE) == LC_COLLATE)
{
if ((_locale->collate = (LPWIN32COLLATE) malloc(sizeof(WIN32COLLATE))) == NULL)
{
SetLastError(ERROR_OUTOFMEMORY);
return NULL;
}
Error *= _setCollate(_locale->handle, _locale->collate);
if (Error != 0)
Error *= _addCategoryName(_locale, "LC_COLLATE");
}
if ((Category & LC_CTYPE) == LC_CTYPE)
{
if ((_locale->ctype = (LPWIN32CTYPE) malloc(sizeof(WIN32CTYPE))) == NULL)
{
SetLastError(ERROR_OUTOFMEMORY);
return NULL;
}
Error *= _setCType(_locale->handle, _locale->codepage, _locale->ctype);
if (Error != 0)
Error *= _addCategoryName(_locale, "LC_CTYPE");
}
return Error;
}
// Private Methods //////////////////////////////////////////////////////////////
void
AP_GPS_NMEA::parse_nmea_gps(void)
{
uint8_t NMEA_check;
long aux_deg;
long aux_min;
char *parseptr;
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
//Serial.println("buffer");
_setTime();
valid_read = true;
new_data = true; // New GPS Data
parseptr = strchr(buffer, ',')+1;
//parseptr = strchr(parseptr, ',')+1;
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
parseptr = strchr(parseptr, ',')+1;
aux_deg = parsedecimal(parseptr, 2); // degrees
aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal
latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5)
parseptr = strchr(parseptr, ',')+1;
if ( * parseptr == 'S')
latitude = -1 * latitude; // South latitudes are negative
parseptr = strchr(parseptr, ',')+1;
// W longitudes are Negative
aux_deg = parsedecimal(parseptr, 3); // degrees
aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal)
longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000)
//longitude = -1*longitude; // This Assumes that we are in W longitudes...
parseptr = strchr(parseptr, ',')+1;
if ( * parseptr == 'W')
longitude = -1 * longitude; // West longitudes are negative
parseptr = strchr(parseptr, ',')+1;
fix = parsedecimal(parseptr, 1);
parseptr = strchr(parseptr, ',')+1;
num_sats = parsedecimal(parseptr, 2);
parseptr = strchr(parseptr, ',')+1;
HDOP = parsenumber(parseptr, 1); // HDOP * 10
parseptr = strchr(parseptr, ',')+1;
altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters
if (fix < 1)
quality = 0; // No FIX
else if(num_sats < 5)
quality = 1; // Bad (Num sats < 5)
else if(HDOP > 30)
quality = 2; // Poor (HDOP > 30)
else if(HDOP > 25)
quality = 3; // Medium (HDOP > 25)
else
quality = 4; // Good (HDOP < 25)
} else {
_error("GPSERR: Checksum error!!\n");
}
}
} else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
//Serial.println(buffer);
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
_setTime();
valid_read = true;
new_data = true; // New GPS Data
parseptr = strchr(buffer, ',')+1;
ground_course = parsenumber(parseptr, 1) * 10; // Ground course in degrees * 100
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
ground_speed = parsenumber(parseptr, 1) * 100 / 36; // Convert Km / h to m / s ( * 100)
//GPS_line = true;
} else {
_error("GPSERR: Checksum error!!\n");
}
}
} else {
bufferidx = 0;
_error("GPSERR: Bad sentence!!\n");
}
}
