static void call_with_arg(void (*f)(int), char *arg, char *expln, char *cmdname)
{
int argnum;
if (arg == NULL)
printf(expln, cmdname);
else if ((argnum = parsenumber(arg)) >= 0)
(*f)(argnum);
}
static Jsonnode* json_parse_endp(const char *str,const char **endp){
Jsonnode *node;
node=parsenumber(str,endp); if(node){SKIPSPACES(*endp); return node;}
node=parsestring(str,endp); if(node){SKIPSPACES(*endp); return node;}
node=parsebool (str,endp); if(node){SKIPSPACES(*endp); return node;}
node=parsenull (str,endp); if(node){SKIPSPACES(*endp); return node;}
node=parsearray (str,endp); if(node){SKIPSPACES(*endp); return node;}
node=parseobject(str,endp); if(node){SKIPSPACES(*endp); return node;}
return NULL;
}
static int
parsevalue(Parser *p, JSON *parent, JSON **prev)
{
switch (*p->s) {
case '{': return parseobject(p, parent, prev);
case '[': return parsearray(p, parent, prev);
case '"': return parsestring(p, parent, prev);
case 't': return parsetrue(p, parent, prev);
case 'f': return parsefalse(p, parent, prev);
case 'n': return parsenull(p, parent, prev);
case '-':
case '0': case '1': case '2': case '3': case '4':
case '5': case '6': case '7': case '8': case '9':
return parsenumber(p, parent, prev);
}
return 0;
}
/**
* Read the input buffer and find any recognized commands. One G or M command per line.
*/
void processCommand() {
// blank lines
if(buffer[0]==';') return;
long cmd;
// is there a line number?
cmd=parsenumber('N',-1);
if(cmd!=-1 && buffer[0]=='N') { // line number must appear first on the line
if( cmd != line_number ) {
// wrong line number error
Serial.print(F("BADLINENUM "));
Serial.println(line_number);
return;
}
// is there a checksum?
if(strchr(buffer,'*')!=0) {
// yes. is it valid?
char checksum=0;
int c=0;
while(buffer[c]!='*') checksum ^= buffer[c++];
c++; // skip *
int against = strtod(buffer+c,NULL);
if( checksum != against ) {
Serial.print(F("BADCHECKSUM "));
Serial.println(line_number);
return;
}
} else {
Serial.print(F("NOCHECKSUM "));
Serial.println(line_number);
return;
}
line_number++;
}
cmd = parsenumber('G',-1);
switch(cmd) {
case 0:
case 1: { // line
feedrate(parsenumber('F',fr));
line( parsenumber('X',(mode_abs?px:0)) + (mode_abs?0:px),
parsenumber('Y',(mode_abs?py:0)) + (mode_abs?0:py) );
break;
}
case 2:
case 3: { // arc
feedrate(parsenumber('F',fr));
arc(parsenumber('I',(mode_abs?px:0)) + (mode_abs?0:px),
parsenumber('J',(mode_abs?py:0)) + (mode_abs?0:py),
parsenumber('X',(mode_abs?px:0)) + (mode_abs?0:px),
parsenumber('Y',(mode_abs?py:0)) + (mode_abs?0:py),
(cmd==2) ? -1 : 1);
break;
}
case 4: pause(parsenumber('S',0) + parsenumber('P',0)*1000.0f); break; // dwell
case 90: mode_abs=1; break; // absolute mode
case 91: mode_abs=0; break; // relative mode
case 92: // set logical position
position( parsenumber('X',0),
parsenumber('Y',0) );
break;
default: break;
}
cmd = parsenumber('M',-1);
switch(cmd) {
case 18: // disable motors
release();
break;
case 100: help(); break;
case 110: line_number = parsenumber('N',line_number); break;
case 114: where(); break;
default: break;
}
}
inline void sinsp_tracerparser::parse_json(char* evtstr)
{
char* p = m_storage;
uint32_t delta;
char* tstr;
//
// Skip the initial bracket
//
m_res = skip_spaces(p, &delta);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
if(*(p++) != '[')
{
m_res = sinsp_tracerparser::RES_FAILED;
return;
}
//
// type
//
m_res = parsestr(p, &m_type_str, &delta);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
//
// ID
//
m_res = skip_spaces_and_commas(p, &delta, 1);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
if(*p == '"')
{
switch(*(++p))
{
case 't':
m_id = m_tinfo->m_tid;
delta = 2;
break;
case 'p':
m_id = m_tinfo->m_pid;
if(*(p + 1) == 'p')
{
m_id = m_tinfo->m_ptid;
p++;
}
delta = 2;
break;
case ':':
m_id = 0;
delta = 1;
break;
case 'g':
m_id = 0;
delta = 2;
break;
default:
m_res = sinsp_tracerparser::RES_FAILED;
break;
}
}
else
{
m_res = parsenumber(p, &m_id, &delta);
if(m_res > sinsp_tracerparser::RES_COMMA)
{
return;
}
}
p += delta;
if(m_res == sinsp_tracerparser::RES_COMMA)
{
m_res = skip_spaces(p, &delta);
}
else
{
m_res = skip_spaces_and_commas(p, &delta, 1);
}
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
//
// First tag
//
m_res = skip_spaces_and_char(p, &delta, '[');
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
m_res = parsestr_not_enforce(p, &tstr, &delta);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
if(tstr != NULL)
{
m_tags.push_back(tstr);
m_taglens.push_back(delta - 2);
m_tot_taglens += delta - 2;
//
// Remaining tags
//
while(true)
{
m_res = skip_spaces_and_commas(p, &delta, 0);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
if(*p == ']')
{
break;
}
m_res = parsestr(p, &tstr, &delta);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
m_tags.push_back(tstr);
m_taglens.push_back(delta - 2);
m_tot_taglens += delta - 2;
}
}
//
// First argument
//
m_res = skip_spaces_and_commas_and_all_brakets(p, &delta);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
m_res = parsestr_not_enforce(p, &tstr, &delta);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
if(tstr != NULL)
{
m_argnames.push_back(tstr);
m_argnamelens.push_back(delta - 2);
m_tot_argnamelens += delta - 2;
m_res = skip_spaces_and_char(p, &delta, ':');
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
m_res = parsestr(p, &tstr, &delta);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
m_argvals.push_back(tstr);
m_argvallens.push_back(delta - 2);
m_tot_argvallens += delta - 2;
//
// Remaining arguments
//
while(true)
{
m_res = skip_spaces_and_commas_and_cr_brakets(p, &delta);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
if(*p == ']')
{
p++;
break;
}
m_res = parsestr(p, &tstr, &delta);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
m_argnames.push_back(tstr);
m_argnamelens.push_back(delta - 2);
m_tot_argnamelens += delta - 2;
m_res = skip_spaces_and_char(p, &delta, ':');
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
m_res = parsestr(p, &tstr, &delta);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
m_argvals.push_back(tstr);
m_argvallens.push_back(delta - 2);
m_tot_argvallens += delta - 2;
}
}
//
// Terminating ]
//
m_res = skip_spaces(p, &delta);
if(m_res != sinsp_tracerparser::RES_OK)
{
return;
}
p += delta;
if(*p != ']')
{
if(*p == 0)
{
m_res = sinsp_tracerparser::RES_TRUNCATED;
}
else
{
m_res = sinsp_tracerparser::RES_FAILED;
}
return;
}
m_res = sinsp_tracerparser::RES_OK;
return;
}
// Private Methods //////////////////////////////////////////////////////////////
void GPS_NMEA_Class::parse_nmea_gps(void)
{
byte 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");
NewData = 1; // 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
Lattitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000) (0.6 = 3/5)
parseptr = strchr(parseptr, ',')+1;
//
if (*parseptr=='S')
Lattitude = -1*Lattitude; // South Lattitudes 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;
NumSats = 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(NumSats<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
{
if (PrintErrors)
Serial.println("GPSERR: Checksum error!!");
}
}
}
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
parseptr = strchr(buffer, ',')+1;
Ground_Course = parsenumber(parseptr,2); // 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,2)*10/36; // Convert Km/h to m/s (*100)
//GPS_line = true;
}
else
{
if (PrintErrors)
Serial.println("GPSERR: Checksum error!!");
}
}
}
else
{
bufferidx = 0;
if (PrintErrors)
Serial.println("GPSERR: Bad sentence!!");
}
}
static inline retvalue parse_data(const char *name, const char *version, const char *data, size_t datalen, /*@out@*/struct trackedpackage **pkg) {
struct trackedpackage *p;
int i;
p = zNEW(struct trackedpackage);
if (FAILEDTOALLOC(p))
return RET_ERROR_OOM;
p->sourcename = strdup(name);
p->sourceversion = strdup(version);
if (FAILEDTOALLOC(p->sourcename)
|| FAILEDTOALLOC(p->sourceversion)
/* || FAILEDTOALLOC(p->sourcedir) */) {
trackedpackage_free(p);
return RET_ERROR_OOM;
}
while (datalen > 0 && *data != '\0') {
char *filekey;
const char *separator;
size_t filekeylen;
retvalue r;
if (((p->filekeys.count)&31) == 0) {
enum filetype *n = realloc(p->filetypes,
(p->filekeys.count+32)*sizeof(enum filetype));
if (FAILEDTOALLOC(n)) {
trackedpackage_free(p);
return RET_ERROR_OOM;
}
p->filetypes = n;
}
p->filetypes[p->filekeys.count] = *data;
data++; datalen--;
separator = memchr(data, '\0', datalen);
if (separator == NULL) {
fprintf(stderr,
"Internal Error: Corrupt tracking data for %s %s\n",
name, version);
trackedpackage_free(p);
return RET_ERROR;
}
filekeylen = separator - data;
filekey = strndup(data, filekeylen);
if (FAILEDTOALLOC(filekey)) {
trackedpackage_free(p);
return RET_ERROR_OOM;
}
r = strlist_add(&p->filekeys, filekey);
if (RET_WAS_ERROR(r)) {
trackedpackage_free(p);
return r;
}
data += filekeylen + 1;
datalen -= filekeylen + 1;
}
data++; datalen--;
p->refcounts = nzNEW(p->filekeys.count, int);
if (FAILEDTOALLOC(p->refcounts)) {
trackedpackage_free(p);
return RET_ERROR_OOM;
}
for (i = 0 ; i < p->filekeys.count ; i++) {
if ((p->refcounts[i] = parsenumber(&data, &datalen)) < 0) {
fprintf(stderr,
"Internal Error: Corrupt tracking data for %s %s\n",
name, version);
trackedpackage_free(p);
return RET_ERROR;
}
}
if (datalen > 0) {
fprintf(stderr,
"Internal Error: Trailing garbage in tracking data for %s %s\n (%ld bytes)",
name, version, (long)datalen);
trackedpackage_free(p);
return RET_ERROR;
}
p->flags.isnew = false;
p->flags.deleted = false;
*pkg = p;
return RET_OK;
}
void processCommand(void) // interpret and store received serial commands
{
// return numbers after char in serialCmd.buff
serialCmd.advancedFlag = parsenumber('A'); // A advanced control
serialCmd.demoFlag = parsenumber('D'); // D demo mode
serialCmd.fingerNum = parsenumber('F'); // F finger number
serialCmd.gripNum = parsenumber('G'); // G predefined grips
serialCmd.handFlag = parsenumber('H'); // H left or right hand
serialCmd.muscleCtrlFlag = parsenumber('M'); // M muscle control
serialCmd.noiseFloor = parsenumber('N'); // N calculate noise floor
serialCmd.stopPos = parsenumber('P'); // P stop position
serialCmd.speed = parsenumber('S'); // S speed
serialCmd.holdTime = parsenumber('T'); // T amount of time for muscle to be tensed for a grip change
serialCmd.sensitivityAdjust = parsenumber('U'); // U update sensitivity offset
serialCmd.instructionsFlag = parsenumber('?'); // ? sends serial instructions
// direction control
if(parsenumber('O') == 0) // O OPEN
serialCmd.direction = OPEN;
else if(parsenumber('C') == 0) // C CLOSE
serialCmd.direction = CLOSE;
else
{ // if a grip is entered without a direction
if(serialCmd.gripNum != BLANK)
{
currentDir = !currentDir;
serialCmd.direction = currentDir; // toggle grip direction
}
}
constrainReceivedValues();
}
// 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");
}
}
/**
* Read the input buffer and find any recognized commands. One G or M command per line.
*/
void processCommand() {
// blank lines
if(buffer[0]==';') return;
long cmd;
// is there a line number?
cmd=parsenumber('N',-1);
if(cmd!=-1 && buffer[0]=='N') { // line number must appear first on the line
if( cmd != line_number ) {
// wrong line number error
Serial.print(F("BADLINENUM "));
Serial.println(line_number);
return;
}
// is there a checksum?
if(strchr(buffer,'*')!=0) {
// yes. is it valid?
char checksum=0;
int c=0;
while(buffer[c]!='*') checksum ^= buffer[c++];
c++; // skip *
int against = strtod(buffer+c,NULL);
if( checksum != against ) {
Serial.print(F("BADCHECKSUM "));
Serial.println(line_number);
return;
}
} else {
Serial.print(F("NOCHECKSUM "));
Serial.println(line_number);
return;
}
line_number++;
}
cmd = parsenumber('G',-1);
switch(cmd) {
case 0: // move linear
case 1: // move linear
// Move Linear here
do_move(parsenumber('Y',0), parsenumber('Z',0), parsenumber('F',30000));
break;
case 3:
delay(parsenumber('D',0));
break;
case 4:
set_exposing_cycles(parsenumber('E',0));
break; // dwell
case 5:
fill_laser_buffer(parsedistance('D',0));
break;
case 6:
delay(20);
expose_line(parsenumber('E',1000));
break;
case 7:
create_test_pattern();
expose_line(parsenumber('E',50000));
break;
case 29:
//home z- axis
home_z_axis();
break;
case 30:
home_y_axis();
break;
case 28:
// home all axis
break;
case 90: mode_abs=1; break; // absolute mode
case 91: mode_abs=0; break; // relative mode
case 92: // set logical position
// set position to 0 here
break;
default: break;
}
// M Code section
cmd = parsenumber('M',-1);
switch(cmd) {
case 18: // disable motors
motors_release();
break;
case 19:
laser_on();
break;
case 20:
laser_off();
break;
case 21:
vat_down();
break;
case 22:
vat_up();
break;
case 100: help(); break;
case 110: line_number = parsenumber('N',line_number); break;
case 114:
// print position here
break;
default: break;
}
}
