Logo Search packages:      
Sourcecode: obdgpslogger version File versions  Download package

void main_loop ( OBDSimPort sp,
struct simsettings ss 
)

It's a main loop.

Parameters:
spthe simport handle
ssthe overall sim state

Definition at line 46 of file mainloop.cc.

References obdsim_generator::clearerrorcodes, ELM_NODATA_PROMPT, ELM_OK_PROMPT, ELM_PROMPT, ELM_QUERY_PROMPT, obdsim_generator::geterrorcodes, obdsim_generator::getvalue, obdsim_generator::idle, obdGetCmdForPID(), obdsim_freezeframes(), OBDSIM_MAXFREEZEFRAMES, OBDSIM_SLEEPTIME, parse_ATcmd(), OBDSimPort::readLine(), render_obdheader(), OBDSimPort::setEcho(), and OBDSimPort::writeData().

                                                       {

      char *line; // Single line from the other end of the device
      char previousline[1024] = "GARBAGE"; // Blank lines mean re-run previous command

      // Benchmarking
      struct timeval benchmarkstart; // Occasionally dump benchmark numbers
      struct timeval benchmarkend; // Occasionally dump benchmark numbers
      int benchmarkcountgood = 0;
      int benchmarkcounttotal = 0;
      float benchmarkdelta; // Time between benchmarkstart and benchmarkend

      const char *newline_cr = "\r";
      const char *newline_crlf = "\r\n";

      sp->setEcho(ss->e_echo);

      int mustexit = 0;

      if(0 != gettimeofday(&benchmarkstart,NULL)) {
            fprintf(stderr, "Couldn't gettimeofday for benchmarking\n");
            mustexit = 1;
      }

      while(!mustexit) {
            // Begin main loop by idling for OBDSIM_SLEEPTIME ms
            struct timeval starttime; // start time through loop
            struct timeval endtime; // end time through loop
            struct timeval selecttime; // =endtime-starttime [for select()]

            struct timeval timeouttime; // Used anytime we need a simulated timeout

            if(0 != gettimeofday(&starttime,NULL)) {
                  perror("Couldn't gettimeofday for sim mainloop starttime");
                  break;
            }


            if(ss->benchmark > 0) {
                  if(0 != gettimeofday(&benchmarkend, NULL)) {
                        fprintf(stderr, "Couldn't gettimeofday for benchmarking\n");
                        break;
                  }
                  benchmarkdelta = (benchmarkend.tv_sec - benchmarkstart.tv_sec) +
                              ((float)(benchmarkend.tv_usec - benchmarkstart.tv_usec))/1000000.0f;
                  if(ss->benchmark <= benchmarkdelta) {
                        printf("%f seconds. %i samples, %i queries. %.2f s/s, %.2f q/s\n",
                              benchmarkdelta,
                              benchmarkcountgood,
                              benchmarkcounttotal,
                              (float)benchmarkcountgood/benchmarkdelta,
                              (float)benchmarkcounttotal/benchmarkdelta);
                        gettimeofday(&benchmarkstart,NULL);
                        benchmarkcountgood = 0;
                        benchmarkcounttotal = 0;
                  }
            }


            int i;
            for(i=0;i<ss->ecu_count;i++) {
                  if(NULL != ss->ecus[i].simgen->idle) {
                        if(0 != ss->ecus[i].simgen->idle(ss->ecus[i].dg,OBDSIM_SLEEPTIME/(ss->ecu_count * 1000))) {
                              mustexit = 1;
                              break;
                        }
                  }
            }

            obdsim_freezeframes(ss->ecus, ss->ecu_count);

            if(0 != gettimeofday(&endtime,NULL)) {
                  perror("Couldn't gettimeofday for sim mainloop endtime");
                  break;
            }

            selecttime.tv_sec = endtime.tv_sec - starttime.tv_sec;
            if (selecttime.tv_sec != 0) {
                        endtime.tv_usec += 1000000*selecttime.tv_sec;
                        selecttime.tv_sec = 0;
            }
            selecttime.tv_usec = (OBDSIM_SLEEPTIME) - (endtime.tv_usec - starttime.tv_usec);
            if(selecttime.tv_usec > 0) {
                  select(0,NULL,NULL,NULL,&selecttime);
            }


            // Now the actual choise-response thing
            line = sp->readLine(); // This is the input line
            char response[1024]; // This is the response

            if(NULL == line) continue;
            if(0 == strlen(line)) {
                  line = previousline;
            } else {
                  strncpy(previousline, line, sizeof(previousline));
            }

            benchmarkcounttotal++;

            for(i=strlen(line)-1;i>=0;i--) { // Strlen is expensive, kids.
                  line[i] = toupper(line[i]);
            }

            // printf("obdsim got request: %s\n", line);

            if(NULL != strstr(line, "EXIT")) {
                  printf("Received EXIT via serial port. Sim Exiting\n");
                  mustexit=1;
                  continue;
            }

            // If we recognised the command
            int command_recognised = 0;

            if('A' == line[0] && 'T' == line[1]) {

                  command_recognised = parse_ATcmd(ss,sp,line,response,sizeof(response));

                  if(0 == command_recognised) {
                        snprintf(response, sizeof(response), "%s", ELM_QUERY_PROMPT);
                  }

                  sp->writeData(ss->e_linefeed?newline_crlf:newline_cr);
                  sp->writeData(response);
                  sp->writeData(ss->e_linefeed?newline_crlf:newline_cr);
                  sp->writeData(ELM_PROMPT);

                  continue;
            }


            int num_vals_read; // Number of values parsed from the sscanf line
            int vals[3]; // Read up to three vals
            num_vals_read = sscanf(line, "%02x %02x %x", &vals[0], &vals[1], &vals[2]);

            int responsecount = 0;

            // Every time we check an ecu, we accumulate time from ecu delays [ms]
            long accumulated_time = 0;
            // Part of accumulating time is finding out how many ECUs have replied
            int ecu_replycount = 0;

            sp->writeData(ss->e_linefeed?newline_crlf:newline_cr);

            // There has *got* to be a better way to do the following complete mess

            if(num_vals_read <= 0) { // Couldn't parse

                  snprintf(response, sizeof(response), "%s", ELM_QUERY_PROMPT);
                  sp->writeData(response);
                  sp->writeData(ss->e_linefeed?newline_crlf:newline_cr);
                  responsecount++;
            } else if(num_vals_read == 1) { // Only got one valid thing [assume it's mode]

                  if(0x03 == vals[0] || 0x07 == vals[0]) { // Get error codes
                        unsigned int errorcodes[20];
                        for(i=0;i<ss->ecu_count;i++) {
                              if(NULL != ss->ecus[i].simgen->geterrorcodes) {
                                    int errorcount;
                                    int mil = 0;
                                    errorcount = ss->ecus[i].simgen->geterrorcodes(ss->ecus[i].dg,
                                          errorcodes, (sizeof(errorcodes)/sizeof(errorcodes[0]))/2, &mil);

                                    if(0 == errorcount) continue;

                                    char header[16] = "\0";
                                    if(ss->e_headers) {
                                          render_obdheader(header, sizeof(header), ss->e_protocol, &ss->ecus[i], 7, ss->e_spaces, ss->e_dlc);
                                    }

                                    int j;
                                    for(j=0;j<errorcount;j+=3) {
                                          char shortbuf[32];
                                          snprintf(shortbuf, sizeof(shortbuf), "%s%02X%s%02X%s%02X%s%02X%s%02X%s%02X",
                                                      ss->e_spaces?" ":"", errorcodes[j*2],
                                                      ss->e_spaces?" ":"", errorcodes[j*2+1],

                                                      ss->e_spaces?" ":"", (errorcount-j)>1?errorcodes[(j+1)*2]:0x00,
                                                      ss->e_spaces?" ":"", (errorcount-j)>1?errorcodes[(j+1)*2+1]:0x00,

                                                      ss->e_spaces?" ":"", (errorcount-j)>2?errorcodes[(j+2)*2]:0x00,
                                                      ss->e_spaces?" ":"", (errorcount-j)>2?errorcodes[(j+2)*2+1]:0x00
                                                      );
                                          // printf("shortbuf: '%s'   i: %i\n", shortbuf, abcd[i]);
                                          snprintf(response, sizeof(response), "%s%02X%s",
                                                      header, 0x43, shortbuf);
                                          sp->writeData(response);
                                          sp->writeData(ss->e_linefeed?newline_crlf:newline_cr);
                                          responsecount++;
                                    }
                              }
                        }
                  } else if(0x04 == vals[0]) { // Reset error codes
                        for(i=0;i<ss->ecu_count;i++) {
                              if(NULL != ss->ecus[i].simgen->clearerrorcodes) {
                                    ss->ecus[i].simgen->clearerrorcodes(ss->ecus[i].dg);
                              }
                        }
                        snprintf(response, sizeof(response), ELM_OK_PROMPT);
                        sp->writeData(response);
                        sp->writeData(ss->e_linefeed?newline_crlf:newline_cr);
                        responsecount++;
                  } else { // Can't do anything with one value, in modes other three or four
                        snprintf(response, sizeof(response), "%s", ELM_QUERY_PROMPT);
                        sp->writeData(response);
                        sp->writeData(ss->e_linefeed?newline_crlf:newline_cr);
                        responsecount++;
                  }
            } else { // Two or more vals  mode0x01 => mode,pid[,possible optimisation]
                                                // mode0x02 => mode,pid,frame

                  struct obdservicecmd *cmd = obdGetCmdForPID(vals[1]);
                  if(NULL == cmd) {
                        snprintf(response, sizeof(response), "%s", ELM_QUERY_PROMPT);
                        sp->writeData(response);
                        sp->writeData(ss->e_linefeed?newline_crlf:newline_cr);
                        responsecount++;
                  } else if(0x02 == vals[0]) {
                        // Freeze frame
                        for(i=0;i<ss->ecu_count;i++) {
                              int frame = 0;
                              if(num_vals_read > 2) {
                                    // Third value is the frame
                                    frame = vals[2];
                              }
                              if(frame < OBDSIM_MAXFREEZEFRAMES && frame <= ss->ecus[i].ffcount) {
                                    // Don't understand frames higher than this
                                    char ffmessage[256] = "\0";
                                    struct freezeframe *ff = &(ss->ecus[i].ff[frame]);
                                    int count = ff->valuecount[vals[1]];
                                    int messagelen = count + 3; // Mode, PID, Frame

                                    // Or could probably just strncat some or something
                                    switch(count) {
                                          case 1:
                                                snprintf(ffmessage, sizeof(ffmessage),"%02X%s%02X%s%02X%s%02X",
                                                      vals[0], ss->e_spaces?" ":"",
                                                      vals[1], ss->e_spaces?" ":"",
                                                      frame, ss->e_spaces?" ":"",
                                                      ss->ecus[i].ff[frame].values[vals[1]][0]);
                                                break;
                                          case 2:
                                                snprintf(ffmessage, sizeof(ffmessage),"%02X%s%02X%s%02X%s%02X%s%02X",
                                                      vals[0], ss->e_spaces?" ":"",
                                                      vals[1], ss->e_spaces?" ":"",
                                                      frame, ss->e_spaces?" ":"",
                                                      ff->values[vals[1]][0], ss->e_spaces?" ":"",
                                                      ff->values[vals[1]][1]);
                                                break;
                                          case 3:
                                                snprintf(ffmessage, sizeof(ffmessage),"%02X%s%02X%s%02X%s%02X%s%02X%s%02X",
                                                      vals[0], ss->e_spaces?" ":"",
                                                      vals[1], ss->e_spaces?" ":"",
                                                      frame, ss->e_spaces?" ":"",
                                                      ff->values[vals[1]][0], ss->e_spaces?" ":"",
                                                      ff->values[vals[1]][1], ss->e_spaces?" ":"",
                                                      ff->values[vals[1]][2]);
                                                break;
                                          case 4:
                                                snprintf(ffmessage, sizeof(ffmessage),"%02X%s%02X%s%02X%s%02X%s%02X%s%02X%s%02X",
                                                      vals[0], ss->e_spaces?" ":"",
                                                      vals[1], ss->e_spaces?" ":"",
                                                      frame, ss->e_spaces?" ":"",
                                                      ff->values[vals[1]][0], ss->e_spaces?" ":"",
                                                      ff->values[vals[1]][1], ss->e_spaces?" ":"",
                                                      ff->values[vals[1]][2], ss->e_spaces?" ":"",
                                                      ff->values[vals[1]][3]);
                                                break;
                                          case 0:
                                          default:
                                                // NO DATA
                                                break;
                                    }
                                    if(count > 0) {
                                          char header[16] = "\0";
                                          if(ss->e_headers) {
                                                render_obdheader(header, sizeof(header), ss->e_protocol, &ss->ecus[i], 7, ss->e_spaces, ss->e_dlc);
                                          }
                                          snprintf(response, sizeof(response), "%s%s", header, ffmessage);
                                          sp->writeData(response);
                                          sp->writeData(ss->e_linefeed?newline_crlf:newline_cr);
                                          responsecount++;
                                    }
                              }

                        }
                  } else if(0x01 != vals[0]) {
                        // Eventually, modes other than 1 should move to the generators
                        //  but for now, respond NO DATA
                  } else {

                        // Here's the meat & potatoes of the whole application

                        for(i=0;i<ss->ecu_count;i++) {
                              unsigned int abcd[8];
                              struct obdgen_ecu *e = ss->ecudelays[i].ecu;

                              if(accumulated_time+ss->ecudelays[i].delay > ss->e_timeout) {
                                    printf("Timeout waiting for ecu %i. [%i > %i]\n",
                                          e->ecu_num, e->customdelay, ss->e_timeout);
                                    continue;
                              }
                              
                              accumulated_time += ss->ecudelays[i].delay;
                              ecu_replycount++;

                              timeouttime.tv_sec=0;
                              timeouttime.tv_usec=1000l*ss->ecudelays[i].delay;
                              select(0,NULL,NULL,NULL,&timeouttime);

                              int count = e->simgen->getvalue(e->dg,
                                                      vals[0], vals[1],
                                                      abcd+0, abcd+1, abcd+2, abcd+3);
                              // fprintf(stderr, "ecu %i count %i\n", i, count);

                              // printf("Returning %i values for %02X %02X\n", count, vals[0], vals[1]);

                              if(-1 == count) {
                                    mustexit = 1;
                                    break;
                              }

                              if(0 < count) {
                                    char header[16] = "\0";
                                    if(ss->e_headers) {
                                          render_obdheader(header, sizeof(header), ss->e_protocol, e, count+2, ss->e_spaces, ss->e_dlc);
                                    }
                                    int j;
                                    char shortbuf[64];
                                    snprintf(response, sizeof(response), "%s%02X%s%02X",
                                                      header,
                                                      vals[0]+0x40, ss->e_spaces?" ":"", vals[1]);
                                    int checksum = 0;
                                    for(j=0;j<count && j<sizeof(abcd)/sizeof(abcd[0]);j++) {
                                          checksum+=abcd[j];
                                          snprintf(shortbuf, sizeof(shortbuf), "%s%02X",
                                                      ss->e_spaces?" ":"", abcd[j]);
                                          // printf("shortbuf: '%s'   j: %i\n", shortbuf, abcd[j]);
                                          strcat(response, shortbuf);
                                    }
                                    if(ss->e_headers) {
                                                            /* &&
                                                (ss->e_protocol->headertype == OBDHEADER_CAN29 ||
                                                ss->e_protocol->headertype == OBDHEADER_CAN11)) { */
                                          snprintf(shortbuf, sizeof(shortbuf), "%s%02X",
                                                      ss->e_spaces?" ":"", checksum&0xFF);
                                          strcat(response, shortbuf);
                                    }

                                    sp->writeData(response);
                                    sp->writeData(ss->e_linefeed?newline_crlf:newline_cr);
                                    responsecount++;
                              }
                        }
                  }
            }

            // Only need a timeout if we didn't get replies from all the ECUs
            if(ecu_replycount<ss->ecu_count || (0x01 == vals[0] && num_vals_read <= 2)) {
                  timeouttime.tv_sec=0;
                  timeouttime.tv_usec=1000l*(ss->e_timeout - accumulated_time);
                  select(0,NULL,NULL,NULL,&timeouttime);
            }
            if(0 >= responsecount) {
                  sp->writeData(ELM_NODATA_PROMPT);
                  sp->writeData(ss->e_linefeed?newline_crlf:newline_cr);
            } else {
                  benchmarkcountgood++;
            }
            sp->writeData(ELM_PROMPT);
      }

      free(ss->device_identifier);

}

Here is the call graph for this function:


Generated by  Doxygen 1.6.0   Back to index