Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_GPS_FILE_ENABLED
       4             : 
       5             : #include "SIM_GPS_FILE.h"
       6             : 
       7             : #include <AP_HAL/AP_HAL.h>
       8             : #include <SITL/SITL.h>
       9             : #include <sys/types.h>
      10             : #include <sys/stat.h>
      11             : #include <fcntl.h>
      12             : 
      13             : extern const AP_HAL::HAL& hal;
      14             : 
      15             : using namespace SITL;
      16             : 
      17             : /*
      18             :   read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED
      19             :  */
      20           0 : void GPS_FILE::publish(const GPS_Data *d)
      21             : {
      22             :     static int fd[2] = {-1,-1};
      23             :     static uint32_t base_time[2];
      24           0 :     const uint16_t lognum = uint16_t(_sitl->gps_log_num.get());
      25           0 :     if (instance > 1) {
      26           0 :         return;
      27             :     }
      28           0 :     if (fd[instance] == -1) {
      29           0 :         char fname[] = "gpsN_NNN.log";
      30           0 :         hal.util->snprintf(fname, 13, "gps%u_%03u.log", instance+1, lognum);
      31           0 :         fd[instance] = open(fname, O_RDONLY|O_CLOEXEC);
      32           0 :         if (fd[instance] == -1) {
      33           0 :             return;
      34             :         }
      35             :     }
      36           0 :     const uint32_t magic = 0x7fe53b04;
      37             :     struct {
      38             :         uint32_t magic;
      39             :         uint32_t time_ms;
      40             :         uint32_t n;
      41             :     } header;
      42           0 :     uint8_t *buf = nullptr;
      43             :     while (true) {
      44           0 :         if (::read(fd[instance], (void *)&header, sizeof(header)) != sizeof(header) ||
      45           0 :             header.magic != magic) {
      46           0 :             goto rewind_file;
      47             :         }
      48           0 :         if (header.time_ms+base_time[instance] > AP_HAL::millis()) {
      49             :             // not ready for this data yet
      50           0 :             ::lseek(fd[instance], -sizeof(header), SEEK_CUR);
      51           0 :             return;
      52             :         }
      53           0 :         buf = NEW_NOTHROW uint8_t[header.n];
      54           0 :         if (buf != nullptr && ::read(fd[instance], buf, header.n) == ssize_t(header.n)) {
      55           0 :             write_to_autopilot((const char *)buf, header.n);
      56           0 :             delete[] buf;
      57           0 :             buf = nullptr;
      58           0 :             continue;
      59             :         }
      60           0 :         goto rewind_file;
      61             :     }
      62             : 
      63           0 : rewind_file:
      64           0 :     ::printf("GPS[%u] rewind\n", unsigned(instance));
      65           0 :     base_time[instance] = AP_HAL::millis();
      66           0 :     ::lseek(fd[instance], 0, SEEK_SET);
      67           0 :     delete[] buf;
      68             : }
      69             : 
      70             : #endif  // AP_SIM_GPS_FILE_ENABLED