diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 816f3e6f2d..3556cbce21 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -98,6 +98,7 @@ private: void _update_gps_nmea(const struct gps_data *d); void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload); void _update_gps_sbp(const struct gps_data *d); + void _update_gps_file(const struct gps_data *d); void _update_gps(double latitude, double longitude, float altitude, double speedN, double speedE, double speedD, bool have_lock); diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 4d3a053d50..039b456960 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -25,6 +25,9 @@ #include #include #include +#include +#include +#include #pragma GCC diagnostic ignored "-Wunused-result" @@ -699,6 +702,31 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d) do_every_count++; } + +/* + temporary method to use file as GPS data + */ +void SITL_State::_update_gps_file(const struct gps_data *d) +{ + static int fd = -1; + if (fd == -1) { + fd = open("/tmp/gps.dat", O_RDONLY); + } + if (fd == -1) { + return; + } + char buf[200]; + ssize_t ret = ::read(fd, buf, sizeof(buf)); + if (ret > 0) { + ::printf("wrote gps %u bytes\n", (unsigned)ret); + _gps_write((const uint8_t *)buf, ret); + } + if (ret == 0) { + ::printf("gps rewind\n"); + lseek(fd, 0, SEEK_SET); + } +} + /* possibly send a new GPS packet */ @@ -802,6 +830,10 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, _update_gps_sbp(&d); break; + case SITL::GPS_TYPE_FILE: + _update_gps_file(&d); + break; + } } diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 517236dbd7..bed0d0ab74 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -47,6 +47,7 @@ public: GPS_TYPE_MTK19 = 4, GPS_TYPE_NMEA = 5, GPS_TYPE_SBP = 6, + GPS_TYPE_FILE = 7 }; struct sitl_fdm state;