static void cmd_led(BaseSequentialStream *chp, int argc, char *argv[])
{
const float r = (argc > 0) ? atoff(argv[0]) : 0;
const float g = (argc > 1) ? atoff(argv[1]) : 0;
const float b = (argc > 2) ? atoff(argv[2]) : 0;
led_set_rgb(r, g, b);
}
/*
GxGGA - name code
Parameter Value Unit Description
UTC hhmmss.sss Universal time coordinated
Lat ddmm.mmmm Latitude
Northing Indicator N=North, S=South
Lon dddmm.mmmm Longitude
Easting Indicator E=East, W=West
Status 0 0=Invalid, 1=2D/3D, 2=DGPS, 6=Dead Reckoning
SVs Used 00 Number of SVs used for Navigation
HDOP 99.99 Horizontal Dilution of Precision
Alt (MSL) m Altitude (above means sea level)
Unit M=Meters
Geoid Sep. m Geoid Separation = Alt(HAE) - Alt(MSL)
Unit M=Meters
Age of DGPS Corr s Age of Differential Corrections
DGPS Ref Station ID of DGPS Reference Station
*/
void nmea_parse_gpgga(char *nmea, loc_t *loc) {
char *p = nmea;
char dStr[GPS_MAX_STRING];
p = strchr(p, ',') + 1; //skip time - we read date&time if Valid in GxRMC
p = strchr(p, ',') + 1; // in p string started with searching address
str_till_comma(p, dStr); // str to float till comma saved modified string
if(strlen(p) == 0) return; // if no data in field - empty data - we return
loc->latitude = atoff(dStr); // fulfil data
p = strchr(p, ',') + 1; // see above
switch (p[0]) {
case 'N':
loc->lat = 'N';
break;
case 'S':
loc->lat = 'S';
break;
case ',':
loc->lat = '\0';
break;
}
p = strchr(p, ',') + 1;
str_till_comma(p, dStr); // str to float till comma saved modified string
loc->longitude = atoff(dStr);
p = strchr(p, ',') + 1;
switch (p[0]) {
case 'W':
loc->lon = 'W';
break;
case 'E':
loc->lon = 'E';
break;
case ',':
loc->lon = '\0';
break;
}
p = strchr(p, ',') + 1;
str_till_comma(p, dStr); // str to float till comma saved modified string
loc->quality = atoi(dStr);
p = strchr(p, ',') + 1;
str_till_comma(p, dStr); // str to float till comma saved modified string
loc->satellites = atoi(dStr);
p = strchr(p, ',') + 1;
p = strchr(p, ',') + 1;
str_till_comma(p, dStr); // str to float till comma saved modified string
loc->altitude = atoff(dStr);
}
static void pinbench(const char *delayStr, const char *onTimeStr, const char *offTimeStr, const char *countStr,
OutputPin* pinParam, brain_pin_e brainPinParam) {
delayMs = atoff(delayStr);
onTime = atoff(onTimeStr);
offTime = atoff(offTimeStr);
count = atoi(countStr);
brainPin = brainPinParam;
pinX = pinParam;
needToRunBench = true;
}
void IMU_DataProcess(IMU* IMU_Structure){
/* Sample frame:
#YPR=-4.25,0.36,-3.73\r\n
*/
static uint8_t CommaCounter;
static uint8_t i;
static uint8_t j; //YawStr[j]
static uint8_t k; //PitchStr
static uint8_t l; //RollStr
static uint8_t m; //AltitudeStr
static char StartProcessingFlag = 0; // Flag, set to 1 when '=' is detected
static char YawStr[6] = {0}; // Temporary char array
static char PitchStr[6] = {0}; // Temporary char array
static char RollStr[6] = {0}; // Temporary char array
static char AltitudeStr[10] = {0}; // Temporary char array
for( i = 0, j = 0, k = 0, l = 0, m = 0, CommaCounter = 0, StartProcessingFlag = 0; i <= 30; i++ ){
if( StartProcessingFlag == 1 ){
if( IMU_DataFrame[i] == '\r')
break;
if(IMU_DataFrame[i] == ',') //increment counter when ',' is detected
CommaCounter++;
/* Parse Yaw angle */
if( (CommaCounter < 1) && (IMU_DataFrame[i] != ',') ){
YawStr[j] = IMU_DataFrame[i];
j++;
}
/* Parse Pitch angle */
else if( CommaCounter == 1 && (IMU_DataFrame[i] != ',') ){
PitchStr[k] = IMU_DataFrame[i];
k++;
}
/* Parse Roll angle */
else if( CommaCounter == 2 && (IMU_DataFrame[i] != ',') ){
RollStr[l] = IMU_DataFrame[i];
l++;
}
}
if( IMU_DataFrame[i] == '=') //set flag to 1 when '=' is detected
StartProcessingFlag = 1;
}
/* Convert ASCII to float and assign to IMU structure */
IMU_Structure->Yaw = atoff(YawStr);
IMU_Structure->Pitch = atoff(PitchStr);
IMU_Structure->Roll = atoff(RollStr);
}
static void setTimingMap(const char * rpmStr, const char *loadStr, const char *valueStr) {
float rpm = atoff(rpmStr);
float engineLoad = atoff(loadStr);
float value = atoff(valueStr);
int rpmIndex = findIndex(config->ignitionRpmBins, IGN_RPM_COUNT, rpm);
rpmIndex = rpmIndex < 0 ? 0 : rpmIndex;
int loadIndex = findIndex(config->ignitionLoadBins, IGN_LOAD_COUNT, engineLoad);
loadIndex = loadIndex < 0 ? 0 : loadIndex;
config->ignitionTable[loadIndex][rpmIndex] = value;
scheduleMsg(&logger, "Setting timing map entry %d:%d to %f", rpmIndex, loadIndex, value);
}
static void setFuelMap(char * rpmStr, char *loadStr, char *valueStr) {
float rpm = atoff(rpmStr);
float engineLoad = atoff(loadStr);
float value = atoff(valueStr);
int rpmIndex = findIndex(engineConfiguration->fuelRpmBins, FUEL_RPM_COUNT, rpm);
rpmIndex = rpmIndex < 0 ? 0 : rpmIndex;
int loadIndex = findIndex(engineConfiguration->fuelLoadBins, FUEL_LOAD_COUNT, engineLoad);
loadIndex = loadIndex < 0 ? 0 : loadIndex;
engineConfiguration->fuelTable[loadIndex][rpmIndex] = value;
scheduleMsg(&logger, "Setting fuel map entry %d:%d to %f", rpmIndex, loadIndex, value);
}
static void setValue(const char *paramStr, const char *valueStr) {
float valueF = atoff(valueStr);
int valueI = atoi(valueStr);
if (strEqualCaseInsensitive(paramStr, "vsscoeff")) {
engineConfiguration->vehicleSpeedCoef = valueF;
#if EFI_PROD_CODE || defined(__DOXYGEN__)
} else if (strEqualCaseInsensitive(paramStr, "alt_t")) {
if (valueI > 10) {
engineConfiguration->alternatorDT = valueI;
}
showAltInfo();
} else if (strEqualCaseInsensitive(paramStr, "alt_offset")) {
engineConfiguration->alternatorOffset = valueI;
} else if (strEqualCaseInsensitive(paramStr, "alt_p")) {
setAltPFactor(valueF);
#endif
} else if (strEqualCaseInsensitive(paramStr, "step1rpm")) {
engineConfiguration->step1rpm = valueI;
} else if (strEqualCaseInsensitive(paramStr, "step1timing")) {
engineConfiguration->step1timing = valueI;
} else if (strEqualCaseInsensitive(paramStr, "targetvbatt")) {
engineConfiguration->targetVBatt = valueF;
#if EFI_RTC || defined(__DOXYGEN__)
} else if (strEqualCaseInsensitive(paramStr, "date")) {
setDateTime(valueStr);
#endif
}
}
static void cmd_rpm(BaseSequentialStream *chp, int argc, char *argv[])
{
static const int TTL_MS = 30000;
if (argc == 0) {
motor_stop();
puts("Usage:\n"
" rpm <RPM>\n"
" rpm arm");
return;
}
// Safety check
static bool _armed = false;
if (!strcmp(argv[0], "arm")) {
_armed = true;
puts("OK");
return;
}
if (!_armed) {
puts("Error: Not armed");
return;
}
long value = (long)atoff(argv[0]);
value = (value < 0) ? 0 : value;
value = (value > 65535) ? 65535 : value;
lowsyslog("RPM %li\n", value);
motor_set_rpm((unsigned)value, TTL_MS);
}
static void cmd_dc(BaseSequentialStream *chp, int argc, char *argv[])
{
static const int TTL_MS = 30000;
if (argc == 0) {
motor_stop();
puts("Usage:\n"
" dc <duty cycle>\n"
" dc arm");
return;
}
// Safety check
static bool _armed = false;
if (!strcmp(argv[0], "arm")) {
_armed = true;
puts("OK");
return;
}
if (!_armed) {
puts("Error: Not armed");
return;
}
const float value = atoff(argv[0]);
lowsyslog("Duty cycle %f\n", value);
motor_set_duty_cycle(value, TTL_MS);
}
float ConfigEntry::as_float() const {
if(type != ENTRY_DATA) {
printf("[ConfigEntry] Trying to read a non-string entry as float\n");
abort();
}
return atoff(entry_string.c_str());
}
qboolean TP_ParseFloat( float *f ) {
const char *token = TP_ParseExt( qfalse );
if ( *token == '\0' ) {
return qtrue;
}
*f = atoff( token );
return qfalse;
}
qboolean TP_ParseFloat( float *f )
{
const char *token = TP_ParseExt( qfalse );
if ( !token[0] )
return qtrue;
*f = atoff( token );
return qfalse;
}
static void cmd_startstop(BaseSequentialStream *chp, int argc, char *argv[])
{
static const int TTL_MS = 5000;
if (argc == 0) {
motor_stop();
puts("Usage:\n"
" startstop <number of cycles> [duty cycle = 0.1]");
return;
}
motor_stop();
const int num_cycles = (int)atoff(argv[0]);
const float dc = (argc > 1) ? atoff(argv[1]) : 0.1;
int current_cycle = 0;
for (; current_cycle < num_cycles; current_cycle++) {
printf("Cycle %d of %d, dc %f...\n", current_cycle + 1, num_cycles, dc);
// Waiting for the motor to spin down
sleep(3);
if (!motor_is_idle()) {
puts("NOT STOPPED");
break;
}
// Starting with the specified duty cycle
motor_set_duty_cycle(dc, TTL_MS);
// Checking if started and stopping
sleep(3);
if (!motor_is_running()) {
puts("NOT RUNNING");
break;
}
motor_stop();
}
printf("Finished %d cycles of %d\n", current_cycle, num_cycles);
}
static void cmd_cfg(BaseSequentialStream *chp, int argc, char *argv[])
{
const char* const command = (argc < 1) ? "" : argv[0];
if (!strcmp(command, "list")) {
for (int i = 0;; i++) {
const char* name = config_name_by_index(i);
if (!name)
break;
const int res = print_param(name, true);
if (res) {
lowsyslog("Internal error %i\n", res);
assert(0);
}
}
}
else if (!strcmp(command, "save") || !strcmp(command, "erase")) {
if (motor_is_idle()) {
const bool save = !strcmp(command, "save");
print_status((save ? config_save : config_erase)());
} else {
puts("I'm sorry Dave, I'm afraid I can't do that");
}
}
else if (!strcmp(command, "get")) {
if (argc < 2) {
puts("Error: Not enough arguments");
return;
}
const int ret = print_param(argv[1], false);
if (ret)
print_status(ret);
}
else if (!strcmp(command, "set")) {
if (argc < 3) {
puts("Error: Not enough arguments");
return;
}
const char* const name = argv[1];
const float value = atoff(argv[2]);
const int res = config_set(name, value);
if (res == 0)
print_param(name, false);
print_status(res);
}
else {
puts("Usage:\n"
" cfg list\n"
" cfg save\n"
" cfg erase\n"
" cfg get <name>\n"
" cfg set <name> <value>");
}
}
qboolean TP_ParseVec4( vector4 *vec ) {
for ( int i = 0; i < 4; i++ ) {
const char *token = TP_ParseExt( qfalse );
if ( *token == '\0' ) {
return qtrue;
}
vec->raw[i] = atoff( token );
}
return qfalse;
}
void cmd_SetImax(char *cmd)
{
int i = 0;
float aux;
// Find first space character. State should be followed by it
while(cmd[i] != ' ' && cmd[i] != '\0') {
i++;
}
aux = atoff(cmd + i);
if((aux >= 0) && (aux <= 6))
Imax = (int)(aux * 4095 * 0.151) / (Vdd * 2);
}
TEST(misc, testMisc) {
print("******************************************* testMisc\r\n");
strcpy(buff, " ab ");
// we need a mutable array here
ASSERT_TRUE(strEqual("ab", efiTrim(buff)));
{
float v = atoff("1.0");
assertEqualsM("atoff", 1.0, v);
}
{
float v = atoff("nan");
ASSERT_TRUE(cisnan(v)) << "NaN atoff";
}
{
float v = atoff("N");
ASSERT_TRUE(cisnan(v)) << "NaN atoff";
}
// ASSERT_EQ(true, strEqual("spa3", getPinName(SPARKOUT_3_OUTPUT)));
// ASSERT_EQ(SPARKOUT_12_OUTPUT, getPinByName("spa12"));
}
void cmd_SetVref(char *cmd)
{
int i = 0;
float aux;
// Find first space character. State should be followed by it
while(cmd[i] != ' ' && cmd[i] != '\0') {
i++;
}
aux = atoff(cmd + i);
aux = (aux * 4095 * 3) /(Vdd * 28);
if((aux >= 0) && (aux <= Vmax))
Vref = (int)aux;
}
qboolean TP_ParseVec4( vector4 *vec )
{
const char *token;
int i;
for ( i=0; i<4; i++ )
{
token = TP_ParseExt( qfalse );
if ( !token[0] )
return qtrue;
vec->data[i] = atoff( token );
}
return qfalse;
}
static void setFloat(const char *offsetStr, const char *valueStr) {
int offset = atoi(offsetStr);
if (absI(offset) == absI(ERROR_CODE)) {
scheduleMsg(&logger, "invalid offset [%s]", offsetStr);
return;
}
if (isOutOfBounds(offset))
return;
float value = atoff(valueStr);
if (cisnan(value)) {
scheduleMsg(&logger, "invalid value [%s]", valueStr);
return;
}
float *ptr = (float *) (&((char *) engine->engineConfiguration)[offset]);
*ptr = value;
getFloat(offset);
}
LEElement *LEElementPool::parseExpression(const char * line) {
LEElement *first = NULL;
LEElement *last = NULL;
while (true) {
line = getNextToken(line, parsingBuffer);
if (line == NULL) {
/**
* No more tokens in this line
*/
return first;
}
LEElement *n = next();
if (isNumeric(parsingBuffer)) {
n->init(LE_NUMERIC_VALUE, atoff(parsingBuffer));
} else {
le_action_e action = parseAction(parsingBuffer);
if (action == LE_UNDEFINED) {
/**
* Cannot recognize token
*/
warning((obd_code_e) 0, "unrecognized [%s]", parsingBuffer);
return NULL;
}
n->init(action);
}
if (first == NULL) {
first = n;
last = n;
} else {
last->next = n;
last = last->next;
}
}
return first;
}
/*
* Verarbeiten der Kommandos die vom Xbee kommen.
*/
void Xbee::CommandoProzess(char *transmission){
int i=0;
int startbyte = 0;
while (transmission[startbyte] != 0x01)
{
startbyte++;
}
int version = transmission[startbyte + 1];
int laenge = transmission[startbyte + 2];
char crc = transmission[startbyte + laenge+1];
int commando;
int paketnummer;
int dataStart;
int dataEnd;
if(transmission[startbyte+laenge+2] == 0x04 )
{
char crc_sum = 0;
int i;
for (i=startbyte+1;i<laenge+1;i++) // CRC von byte 1 bis zum CRC byte (ohne CRC)
{
crc_sum += transmission[i];
}
if (crc_sum == crc )
{
if (version == XBEE_PROTOKOLL_VERSION)
{
commando = transmission[startbyte + 4];
paketnummer = transmission[startbyte + 5];
dataStart = startbyte + 6;
dataEnd = startbyte + laenge;
}
else
{
TerminalInstance->SendMessage("\n\rDebug: Unknown version number->\n\r");
return;
}
}
else
{
TerminalInstance->SendMessage("\n\rDebug: CRC is wrong\n\r");
TerminalInstance->SendViaDma(transmission,laenge + 2);
return;
}
}
else
{
TerminalInstance->SendMessage("\n\rDebug: End byte not right->\n\r");
TerminalInstance->SendViaDma(transmission,laenge + 2);
return;
}
if (commando == 0x10)
{
DisplayInstance->SendByte(transmission + dataStart, dataEnd - dataStart+1, DISPLAY_SOURCE_LOCAL);
}
else if (commando == XBEE_COM_SET_SOLLWERT_VALUE)
{
for (i=0;i<dataEnd - dataStart+1;i++)
{
bufferX[i] = *(transmission + dataStart + i);
}
bufferX[dataEnd - dataStart+2] = 0x00;
FassadeInstance->SetSolltemp(atoff(bufferX));
}
else if (commando == XBEE_COM_VALUE_SOLLWERT)
{
for (i=0;i<dataEnd - dataStart+1;i++)
{
bufferX[i] = *(transmission + dataStart + i);
}
bufferX[dataEnd - dataStart+2] = 0x00;
XbeeInstance->values.sollwert = atoff(bufferX);
//MenuInstance->SetSollwert(atoff(bufferX));
}
else if (commando == XBEE_COM_VALUE_INDOOR)
{
for (i=0;i<dataEnd - dataStart+1;i++)
{
bufferX[i] = *(transmission + dataStart + i);
}
bufferX[dataEnd - dataStart+2] = 0x00;
XbeeInstance->values.indor = atoff(bufferX);
}
else if (commando == XBEE_COM_VALUE_OUTDOOR)
{
for (i=0;i<dataEnd - dataStart+1;i++)
{
bufferX[i] = *(transmission + dataStart + i);
}
bufferX[dataEnd - dataStart+2] = 0x00;
XbeeInstance->values.outdor = atoff(bufferX);
}
else if (commando == XBEE_COM_VALUE_CURRENT)
{
for (i=0;i<dataEnd - dataStart+1;i++)
{
bufferX[i] = *(transmission + dataStart + i);
}
bufferX[dataEnd - dataStart+2] = 0x00;
XbeeInstance->values.current = atoff(bufferX);
}
else if (commando == XBEE_COM_VALUE_VOLTAGE)
{
for (i=0;i<dataEnd - dataStart+1;i++)
{
bufferX[i] = *(transmission + dataStart + i);
}
bufferX[dataEnd - dataStart+2] = 0x00;
XbeeInstance->values.voltage = atoff(bufferX);
}
else if (commando == XBEE_COM_VALUE_WINDOW)
{
for (i=0;i<dataEnd - dataStart+1;i++)
{
bufferX[i] = *(transmission + dataStart + i);
}
bufferX[dataEnd - dataStart+2] = 0x00;
XbeeInstance->values.windowP = atoi(bufferX);
}
else if (commando == XBEE_COM_INFO_DEFAULT_SCREEN)
{
menuShow = false;
FassadeInstance->UpdateDisplayValues();
}
else if (commando == XBEE_COM_GETVALUE)
{
switch(*(transmission + dataStart ))
{
case XBEE_COM_VALUE_SOLLWERT:
{
sprintf(bufferX,"%f",FassadeInstance->GetSolltemp());
XbeeInstance->SendTransmission(XBEE_PROTOKOLL_VERSION, XBEE_TYPE_REMOUTE,
XBEE_COM_VALUE_SOLLWERT,(char) 0x00, bufferX ,(char)strlen(bufferX));
}
default:;
}
}
}
void parseCSV() {
char time[MAX_STRING];
float ig = 0, bg = 0, mod = 0;
int timePos = 0, igPos = 0, bgPos = 0, modPos = 0;
int column = 0;
int length = 0;
char *pos = NULL;
char *first = NULL;
data = (LPAPPROXPOINT) malloc(sizeof (APPROXPOINT) * MAX_LINES);
if (data == NULL) {
printf("Memory could not be allocated!\n");
exit(1);
}
if ((f = fopen("A0904.csv", "r")) == NULL) {
printf("File could not be opened!\n");
exit(1);
}
char line[1024];
char tmp[500];
while (fgets(line, BUFFER_SIZE, f) != NULL) {
int i;
if (count == 0) {
first = line;
pos = line;
column = 0;
for (i = 0; i < strlen(line); i++) {
if (line[i] != ';' && line[i] != '\n') {
pos++;
} else {
length = pos - first;
strncpy(tmp, first, length);
if (tmp[length - 1] == '\r') {
tmp[length - 1] = '\0';
} else {
tmp[length] = '\0';
}
if (strcmp(tmp, TIME) == 0) {
timePos = column;
} else if (strcmp(tmp, BG) == 0) {
bgPos = column;
} else if (strcmp(tmp, IG) == 0) {
igPos = column;
} else if (strcmp(tmp, MOD) == 0) {
modPos = column;
} else {
}
first = ++pos;
column++;
}
}
count++;
} else {
first = line;
pos = line;
column = 0;
for (i = 0; i < strlen(line); i++) {
if (line[i] == ',') {
line[i] = '.';
}
if (line[i] != ';' && line[i] != '\n') {
pos++;
} else {
length = pos - first;
strncpy(tmp, first, length);
if (tmp[length - 1] == '\r') {
tmp[length - 1] = '\0';
} else {
tmp[length] = '\0';
}
first = ++pos;
if (column == timePos) {
strncpy(time, tmp, strlen(tmp) + 1);
} else if (column == bgPos) {
bg = atoff(tmp);
} else if (column == igPos) {
ig = atoff(tmp);
} else if (column == modPos) {
mod = atoff(tmp);
} else {
}
column++;
}
}
if (bg != 0 && ig != 0) {
strncpy(data[count - 1].time, time, strlen(time));
data[count - 1].bg = bg;
data[count - 1].ig = ig;
data[count - 1].mod = mod;
/*printf("%s %f %f %f\n", data[count - 1].time, data[count - 1].bg,
data[count - 1].ig, data[count - 1].mod);*/
if (((int) ig) < minI) {
minI = (int) ig;
}
if (((int) ig) > maxI) {
maxI = (int) ig;
}
if (((int) bg) < minB) {
minB = (int) bg;
}
if (((int) bg) > maxB) {
maxB = (int) bg;
}
count++;
}
}
}
count--;
printf("Nacteno %i radku\n", count);
fclose(f);
}
void GPS_UAVParseGGA(GPS* GPS_Structure){
if( (UAV_DataFrame[3] == 'G') && (UAV_DataFrame[4] == 'G') && (UAV_DataFrame[5] == 'A') ){
static uint8_t i = 0; //UAV_DataFrame[i]
static uint8_t j = 0; //LatitudeStr[j]
static uint8_t k = 0; //LongitudeStr[k]
static uint8_t l = 0; //AltitudeStr[l]
static uint8_t m = 0; //TimeStr[m]
uint8_t CommaCounter= 0;
char TimeStr[20] = {0}; // Temporary char array
char LatitudeStr[20] = {0}; // Temporary char array
char LongitudeStr[20] = {0}; // Temporary char array
char AltitudeStr[20] = {0}; // Temporary char array
for( i =0, j =0, k=0, l =0, m =0 ; i < 100; i++ ){
if( UAV_DataFrame[i] == ',' )
CommaCounter++;
if( (UAV_DataFrame[i] != ',') && (CommaCounter == 1)){
TimeStr[m] = UAV_DataFrame[i];
m++;
}
if( (UAV_DataFrame[i] != ',') && (CommaCounter == 2)){
LatitudeStr[j] = UAV_DataFrame[i];
j++;
}
if( (UAV_DataFrame[i] != ',') && (CommaCounter == 4) ){
LongitudeStr[k] = UAV_DataFrame[i];
k++;
}
if( (UAV_DataFrame[i] != ',') && (CommaCounter == 9) ){
AltitudeStr[l] = UAV_DataFrame[i];
l++;
}
if( CommaCounter == 2 ){
char TmpStr[2] = {TimeStr[0], TimeStr[1]};
GPS_Structure->Time_hours = atoi(TmpStr);
TmpStr[0] = TimeStr[2];
TmpStr[1] = TimeStr[3];
GPS_Structure->Time_minutes = atoi(TmpStr);
TmpStr[0] = TimeStr[4];
TmpStr[1] = TimeStr[5];
GPS_Structure->Time_seconds = atoi(TmpStr);
}
/* Parsowanie Latitude */
if( CommaCounter == 3 ){
char TmpLatD[2] = {LatitudeStr[0], LatitudeStr[1]};
GPS_Structure->Latitude_degrees = atoi(TmpLatD);
uint8_t tmp = strlen(LatitudeStr);
char TmpLatM[tmp-2];
for(j = 2; j < tmp; j++){
TmpLatM[j-2] = LatitudeStr[j];
}
GPS_Structure->Latitude_minutes = atoff(TmpLatM);
GPS_Structure->Latitude = atoff(LatitudeStr);
GPS_Structure->Latitude_decimal = GPS_Structure->Latitude_degrees + (0.016666667 * GPS_Structure->Latitude_minutes);
}
/* Parsowanie Longitude */
if( CommaCounter == 5 ){
char TmpLonD[2] = {LongitudeStr[1], LongitudeStr[2]};
GPS_Structure->Longitude_degrees = atoi(TmpLonD);
uint8_t tmp = strlen(LongitudeStr);
char TmpLonM[tmp-3];
for(k = 3; k < tmp; k++){
TmpLonM[k-3] = LongitudeStr[k];
}
GPS_Structure->Longitude_minutes = atoff(TmpLonM);
GPS_Structure->Longitude = atoff(LongitudeStr);
GPS_Structure->Longitude_decimal = GPS_Structure->Longitude_degrees + (0.016666667 * GPS_Structure->Longitude_minutes);
}
if( CommaCounter >= 10){
GPS_Structure->Altitude = atoff(AltitudeStr);
break;
}
}
}
}
void handleActionWithParameter(TokenCallback *current, char *parameter) {
if (current->parameterType == STRING_PARAMETER) {
VoidCharPtr callbackS = (VoidCharPtr) current->callback;
(*callbackS)(parameter);
return;
}
if (current->parameterType == STRING_PARAMETER_P) {
VoidCharPtrVoidPtr callbackS = (VoidCharPtrVoidPtr) current->callback;
(*callbackS)(parameter, current->param);
return;
}
// todo: refactor this hell!
if (current->parameterType == STRING2_PARAMETER || current->parameterType == STRING2_PARAMETER_P) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1) {
return;
}
parameter[spaceIndex] = 0;
char * param0 = parameter;
parameter += spaceIndex + 1;
char * param1 = parameter;
if (current->parameterType == STRING2_PARAMETER) {
VoidCharPtrCharPtr callbackS = (VoidCharPtrCharPtr) current->callback;
(*callbackS)(param0, param1);
} else {
VoidCharPtrCharPtrVoidPtr callbackS = (VoidCharPtrCharPtrVoidPtr) current->callback;
(*callbackS)(param0, param1, current->param);
}
return;
}
if (current->parameterType == STRING3_PARAMETER) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1) {
return;
}
parameter[spaceIndex] = 0;
char * param0 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
parameter[spaceIndex] = 0;
char * param1 = parameter;
parameter += spaceIndex + 1;
char * param2 = parameter;
VoidCharPtrCharPtrCharPtr callbackS = (VoidCharPtrCharPtrCharPtr) current->callback;
(*callbackS)(param0, param1, param2);
return;
}
// todo: refactor this hell!
if (current->parameterType == STRING5_PARAMETER) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1) {
return;
}
parameter[spaceIndex] = 0;
char * param0 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
parameter[spaceIndex] = 0;
char * param1 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
parameter[spaceIndex] = 0;
char * param2 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
parameter[spaceIndex] = 0;
char * param3 = parameter;
parameter += spaceIndex + 1;
char * param4 = parameter;
VoidCharPtrCharPtrCharPtrCharPtrCharPtr callbackS = (VoidCharPtrCharPtrCharPtrCharPtrCharPtr) current->callback;
(*callbackS)(param0, param1, param2, param3, param4);
return;
}
if (current->parameterType == TWO_INTS_PARAMETER) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
parameter[spaceIndex] = 0;
int value1 = atoi(parameter);
if (absI(value1) == ERROR_CODE) {
#if (EFI_PROD_CODE || EFI_SIMULATOR) || defined(__DOXYGEN__)
scheduleMsg(logging, "not an integer [%s]", parameter);
#endif
return;
}
parameter += spaceIndex + 1;
int value2 = atoi(parameter);
if (absI(value2) == ERROR_CODE) {
#if (EFI_PROD_CODE || EFI_SIMULATOR) || defined(__DOXYGEN__)
scheduleMsg(logging, "not an integer [%s]", parameter);
#endif
return;
}
VoidIntInt callbackS = (VoidIntInt) current->callback;
(*callbackS)(value1, value2);
return;
}
if (current->parameterType == FLOAT_PARAMETER) {
float value = atoff(parameter);
VoidFloat callbackF = (VoidFloat) current->callback;
// invoke callback function by reference
(*callbackF)(value);
return;
}
if (current->parameterType == FLOAT_FLOAT_PARAMETER || current->parameterType == FLOAT_FLOAT_PARAMETER_P) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
parameter[spaceIndex] = 0;
float value1 = atoff(parameter);
parameter += spaceIndex + 1;
float value2 = atoff(parameter);
if (current->parameterType == FLOAT_FLOAT_PARAMETER) {
VoidFloatFloat callbackS = (VoidFloatFloat) current->callback;
(*callbackS)(value1, value2);
} else {
VoidFloatFloatVoidPtr callbackS = (VoidFloatFloatVoidPtr) current->callback;
(*callbackS)(value1, value2, current->param);
}
return;
}
int value = atoi(parameter);
if (absI(value) == ERROR_CODE) {
print("invalid integer [%s]\r\n", parameter);
return;
}
if (current->parameterType == ONE_PARAMETER_P) {
VoidIntVoidPtr callback1 = (VoidIntVoidPtr) current->callback;
// invoke callback function by reference
(*callback1)(value, current->param);
} else {
VoidInt callback1 = (VoidInt) current->callback;
// invoke callback function by reference
(*callback1)(value);
}
}
void parse_command_line(int argc, char **argv, char *input_file_name, char *model_file_name)
{
int i;
// default values
param.solver_type = L2R_L2LOSS_SVC_DUAL;
param.C = 1;
param.eps = INF; // see setting below
param.nr_weight = 0;
param.weight_label = NULL;
param.weight = NULL;
flag_cross_validation = 0;
bias = -1;
// parse options
for(i=1;i<argc;i++)
{
if(argv[i][0] != '-') break;
if(++i>=argc)
exit_with_help();
switch(argv[i-1][1])
{
case 's':
param.solver_type = atoi(argv[i]);
break;
case 'c':
param.C = atoff(argv[i]);
break;
case 'e':
param.eps = atoff(argv[i]);
break;
case 'B':
bias = atoff(argv[i]);
break;
case 'w':
++param.nr_weight;
param.weight_label = (int *) realloc(param.weight_label,sizeof(int)*param.nr_weight);
param.weight = (float *) realloc(param.weight,sizeof(double)*param.nr_weight);
param.weight_label[param.nr_weight-1] = atoi(&argv[i-1][2]);
param.weight[param.nr_weight-1] = atoff(argv[i]);
break;
case 'v':
flag_cross_validation = 1;
nr_fold = atoi(argv[i]);
if(nr_fold < 2)
{
fprintf(stderr,"n-fold cross validation: n must >= 2\n");
exit_with_help();
}
break;
case 'q':
liblinear_print_string = &print_null;
i--;
break;
default:
fprintf(stderr,"unknown option: -%c\n", argv[i-1][1]);
exit_with_help();
break;
}
}
// determine filenames
if(i>=argc)
exit_with_help();
strcpy(input_file_name, argv[i]);
if(i<argc-1)
strcpy(model_file_name,argv[i+1]);
else
{
char *p = strrchr(argv[i],'/');
if(p==NULL)
p = argv[i];
else
++p;
sprintf(model_file_name,"%s.model",p);
}
if(param.eps == INF)
{
if(param.solver_type == L2R_LR || param.solver_type == L2R_L2LOSS_SVC)
param.eps = 0.01;
else if(param.solver_type == L2R_L2LOSS_SVC_DUAL || param.solver_type == L2R_L1LOSS_SVC_DUAL || param.solver_type == MCSVM_CS)
param.eps = 0.1;
else if(param.solver_type == L1R_L2LOSS_SVC || param.solver_type == L1R_LR)
param.eps = 0.01;
}
}
void handleActionWithParameter(TokenCallback *current, char *parameter) {
while (parameter[0] == SPACE_CHAR) {
parameter[0] = 0;
parameter++;
}
switch (current->parameterType) {
case STRING_PARAMETER:
{
VoidCharPtr callbackS = (VoidCharPtr) current->callback;
(*callbackS)(parameter);
return;
}
case STRING_PARAMETER_P:
{
VoidCharPtrVoidPtr callbackS = (VoidCharPtrVoidPtr) current->callback;
(*callbackS)(parameter, current->param);
return;
}
default:
// todo: handle all cases explicitly
break;
}
// todo: refactor this hell!
if (current->parameterType == STRING2_PARAMETER || current->parameterType == STRING2_PARAMETER_P) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1) {
return;
}
REPLACE_SPACES_WITH_ZERO;
char * param0 = parameter;
parameter += spaceIndex + 1;
char * param1 = parameter;
if (current->parameterType == STRING2_PARAMETER) {
VoidCharPtrCharPtr callbackS = (VoidCharPtrCharPtr) current->callback;
(*callbackS)(param0, param1);
} else {
VoidCharPtrCharPtrVoidPtr callbackS = (VoidCharPtrCharPtrVoidPtr) current->callback;
(*callbackS)(param0, param1, current->param);
}
return;
}
if (current->parameterType == STRING3_PARAMETER) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1) {
return;
}
REPLACE_SPACES_WITH_ZERO;
char * param0 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
char * param1 = parameter;
parameter += spaceIndex + 1;
char * param2 = parameter;
VoidCharPtrCharPtrCharPtr callbackS = (VoidCharPtrCharPtrCharPtr) current->callback;
(*callbackS)(param0, param1, param2);
return;
}
// todo: refactor this hell!
if (current->parameterType == STRING5_PARAMETER) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1) {
return;
}
REPLACE_SPACES_WITH_ZERO;
char * param0 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
char * param1 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
char * param2 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
char * param3 = parameter;
parameter += spaceIndex + 1;
char * param4 = parameter;
VoidCharPtrCharPtrCharPtrCharPtrCharPtr callbackS = (VoidCharPtrCharPtrCharPtrCharPtrCharPtr) current->callback;
(*callbackS)(param0, param1, param2, param3, param4);
return;
}
if (current->parameterType == TWO_INTS_PARAMETER) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
int value1 = atoi(parameter);
if (absI(value1) == ERROR_CODE) {
#if (EFI_PROD_CODE || EFI_SIMULATOR) || defined(__DOXYGEN__)
scheduleMsg(logging, "not an integer [%s]", parameter);
#endif
return;
}
parameter += spaceIndex + 1;
int value2 = atoi(parameter);
if (absI(value2) == ERROR_CODE) {
#if (EFI_PROD_CODE || EFI_SIMULATOR) || defined(__DOXYGEN__)
scheduleMsg(logging, "not an integer [%s]", parameter);
#endif
return;
}
VoidIntInt callbackS = (VoidIntInt) current->callback;
(*callbackS)(value1, value2);
return;
}
if (current->parameterType == FLOAT_PARAMETER_NAN_ALLOWED) {
float value = atoff(parameter);
VoidFloat callbackF = (VoidFloat) current->callback;
// invoke callback function by reference
(*callbackF)(value);
return;
}
if (current->parameterType == FLOAT_PARAMETER) {
float value = atoff(parameter);
if (cisnan(value)) {
print("invalid float [%s]\r\n", parameter);
return;
}
VoidFloat callbackF = (VoidFloat) current->callback;
// invoke callback function by reference
(*callbackF)(value);
return;
}
if (current->parameterType == FLOAT_FLOAT_PARAMETER || current->parameterType == FLOAT_FLOAT_PARAMETER_P) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
float value1 = atoff(parameter);
if (cisnan(value1)) {
print("invalid float [%s]\r\n", parameter);
return;
}
parameter += spaceIndex + 1;
float value2 = atoff(parameter);
if (cisnan(value2)) {
print("invalid float [%s]\r\n", parameter);
return;
}
if (current->parameterType == FLOAT_FLOAT_PARAMETER) {
VoidFloatFloat callbackS = (VoidFloatFloat) current->callback;
(*callbackS)(value1, value2);
} else {
VoidFloatFloatVoidPtr callbackS = (VoidFloatFloatVoidPtr) current->callback;
(*callbackS)(value1, value2, current->param);
}
return;
}
int value = atoi(parameter);
if (absI(value) == ERROR_CODE) {
print("invalid integer [%s]\r\n", parameter);
return;
}
if (current->parameterType == ONE_PARAMETER_P) {
VoidIntVoidPtr callback1 = (VoidIntVoidPtr) current->callback;
// invoke callback function by reference
(*callback1)(value, current->param);
} else {
VoidInt callback1 = (VoidInt) current->callback;
// invoke callback function by reference
(*callback1)(value);
}
}
void handleActionWithParameter(TokenCallback *current, char *parameter) {
if (current->parameterType == STRING_PARAMETER) {
VoidCharPtr callbackS = (VoidCharPtr) current->callback;
(*callbackS)(parameter);
return;
}
if (current->parameterType == STRING3_PARAMETER) {
int spaceIndex = indexOf(parameter, ' ');
if (spaceIndex == -1)
return;
parameter[spaceIndex] = 0;
char * firstParam = parameter;
parameter += spaceIndex + 1;
spaceIndex = indexOf(parameter, ' ');
if (spaceIndex == -1)
return;
parameter[spaceIndex] = 0;
char * secondParam = parameter;
parameter += spaceIndex + 1;
char * thirdParam = parameter;
VoidCharPtrCharPtrCharPtr callbackS = (VoidCharPtrCharPtrCharPtr) current->callback;
(*callbackS)(firstParam, secondParam, thirdParam);
return;
}
if (current->parameterType == TWO_INTS_PARAMETER) {
int spaceIndex = indexOf(parameter, ' ');
if (spaceIndex == -1)
return;
parameter[spaceIndex] = 0;
int value1 = atoi(parameter);
parameter += spaceIndex + 1;
int value2 = atoi(parameter);
VoidIntInt callbackS = (VoidIntInt) current->callback;
(*callbackS)(value1, value2);
return;
}
if (current->parameterType == FLOAT_PARAMETER) {
float value = atoff(parameter);
VoidFloat callbackF = (VoidFloat) current->callback;
// invoke callback function by reference
(*callbackF)(value);
return;
}
int value = atoi(parameter);
if (value == ERROR_CODE) {
print("invalid integer [%s]\r\n", parameter);
return;
}
VoidInt callback1 = (VoidInt) current->callback;
// invoke callback function by reference
(*callback1)(value);
}
/*
GxRMC - nmea code
Parameter Value Unit Description
UTC hhmmss.sss Universal time coordinated
Status V A=Valid, V=Invalid
Lat ddmm.mmmm Latitude
Northing Indicator N=North, S=South
Lon dddmm.mmmm Longitude
Easting Indicator E=East, W=West
SOG nots Speed Over Ground
COG (true) ° Course Over Ground (true)
Date ddmmyy Universal time coordinated
Magnetic Variation ° Magnetic Variation
Magnetic Variation E=East,W=West
Mode Indicator N A=Autonomous, D=Differential, E=Dead Reckoning, N=None
Navigational Status S=Safe C=Caution U=Unsafe V=Not valid
*/
void nmea_parse_gprmc(char *nmea, loc_t *loc) {
char *p = nmea;
char dStr[GPS_MAX_STRING];
struct tm timp;
p = strchr(p, ',') + 1; //read time
str_till_comma(p, dStr);
if(strlen(dStr) > 5){
timp.tm_hour = str2int(dStr,2);
timp.tm_min = str2int(dStr+2,2);
timp.tm_sec = str2int(dStr+4,2);
}
p = strchr(p, ',') + 1; //read field Valid status
str_till_comma(p, dStr);
if(dStr[0] == 'V') { // if field is invalid
loc->quality = 0;
return;
}
loc->quality = 4; // this is declaration that last receive field VALID
p = strchr(p, ',') + 1; // latitude
str_till_comma(p, dStr); // str to float till comma saved modified string
loc->latitude = atoff(dStr);
p = strchr(p, ',') + 1;
switch (p[0]) {
case 'N':
loc->lat = 'N';
break;
case 'S':
loc->lat = 'S';
break;
case ',':
loc->lat = '\0';
break;
}
p = strchr(p, ',') + 1; // longitude
str_till_comma(p, dStr); // str to float till comma saved modified string
loc->longitude = atoff(dStr);
p = strchr(p, ',') + 1;
switch (p[0]) {
case 'W':
loc->lon = 'W';
break;
case 'E':
loc->lon = 'E';
break;
case ',':
loc->lon = '\0';
break;
}
p = strchr(p, ',') + 1;
str_till_comma(p, dStr); // str to float till comma saved modified string
loc->speed = atoff(dStr);
p = strchr(p, ',') + 1;
str_till_comma(p, dStr); // str to float till comma saved modified string
loc->course = atoff(dStr);
p = strchr(p, ',') + 1; //read date
str_till_comma(p, dStr);
if(strlen(dStr) > 5){
timp.tm_mday = str2int(dStr,2);
timp.tm_mon = str2int(dStr+2,2);
timp.tm_year = str2int(dStr+4,2)+100; // we receive -200, but standard wait -1900 = add correction
}
if( timp.tm_year > 0 ) { // check if date field is valid
memcpy(&loc->GPStm, &timp, sizeof(timp));
}
}
void
_DEFUN_VOID(test_atoff)
{
test_mok(atoff(pd->string), pd->value, 32);
}
