From a9aa5c2d60aa967ecc851aa9da0af6d3f346af19 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 15 Jan 2022 15:26:06 +1100 Subject: [PATCH] AP_GPS: add a new AP_GPS_SITL object --- libraries/AP_GPS/AP_GPS.cpp | 10 +++ libraries/AP_GPS/AP_GPS.h | 4 ++ libraries/AP_GPS/AP_GPS_SITL.cpp | 120 +++++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_SITL.h | 40 +++++++++++ 4 files changed, 174 insertions(+) create mode 100644 libraries/AP_GPS/AP_GPS_SITL.cpp create mode 100644 libraries/AP_GPS/AP_GPS_SITL.h diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 3c9ede363e..d794aa1e34 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -37,6 +37,9 @@ #include "AP_GPS_MSP.h" #include "AP_GPS_ExternalAHRS.h" #include "GPS_Backend.h" +#if HAL_SIM_GPS_ENABLED +#include "AP_GPS_SITL.h" +#endif #if HAL_ENABLE_LIBUAVCAN_DRIVERS #include @@ -655,6 +658,13 @@ void AP_GPS::detect_instance(uint8_t instance) new_gps = new AP_GPS_NOVA(*this, state[instance], _port[instance]); break; #endif //AP_GPS_NOVA_ENABLED + +#if HAL_SIM_GPS_ENABLED + case GPS_TYPE_SITL: + new_gps = new AP_GPS_SITL(*this, state[instance], _port[instance]); + break; +#endif // HAL_SIM_GPS_ENABLED + default: break; } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 89411ac9b5..65c37b2a68 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -22,6 +22,7 @@ #include "GPS_detect_state.h" #include #include +#include /** maximum number of GPS instances available on this platform. If more @@ -127,6 +128,9 @@ public: GPS_TYPE_EXTERNAL_AHRS = 21, GPS_TYPE_UAVCAN_RTK_BASE = 22, GPS_TYPE_UAVCAN_RTK_ROVER = 23, +#if HAL_SIM_GPS_ENABLED + GPS_TYPE_SITL = 100, +#endif }; /// GPS status codes diff --git a/libraries/AP_GPS/AP_GPS_SITL.cpp b/libraries/AP_GPS/AP_GPS_SITL.cpp new file mode 100644 index 0000000000..fda48fb69a --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_SITL.cpp @@ -0,0 +1,120 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_GPS_SITL.h" + +#if HAL_SIM_GPS_ENABLED + +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +/* + return GPS time of week in milliseconds + */ +/* + get timeval using simulation time + */ +static void simulation_timeval(struct timeval *tv) +{ + uint64_t now = AP_HAL::micros64(); + static uint64_t first_usec; + static struct timeval first_tv; + if (first_usec == 0) { + first_usec = now; + first_tv.tv_sec = AP::sitl()->start_time_UTC; + } + *tv = first_tv; + tv->tv_sec += now / 1000000ULL; + uint64_t new_usec = tv->tv_usec + (now % 1000000ULL); + tv->tv_sec += new_usec / 1000000ULL; + tv->tv_usec = new_usec % 1000000ULL; +} +static void gps_time(uint16_t *time_week, uint32_t *time_week_ms) +{ + struct timeval tv; + simulation_timeval(&tv); + const uint32_t epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - (GPS_LEAPSECONDS_MILLIS / 1000ULL); + uint32_t epoch_seconds = tv.tv_sec - epoch; + *time_week = epoch_seconds / AP_SEC_PER_WEEK; + uint32_t t_ms = tv.tv_usec / 1000; + // round time to nearest 200ms + *time_week_ms = (epoch_seconds % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC + ((t_ms/200) * 200); +} + +bool AP_GPS_SITL::read(void) +{ + const uint32_t now = AP_HAL::millis(); + if (now - last_update_ms < 200) { + return false; + } + last_update_ms = now; + + auto *sitl = AP::sitl(); + + double latitude =sitl->state.latitude; + double longitude = sitl->state.longitude; + float altitude = sitl->state.altitude; + const double speedN = sitl->state.speedN; + const double speedE = sitl->state.speedE; + const double speedD = sitl->state.speedD; + // const double yaw = sitl->state.yawDeg; + + uint16_t time_week; + uint32_t time_week_ms; + + gps_time(&time_week, &time_week_ms); + + state.time_week = time_week; + state.time_week_ms = time_week_ms; + state.status = AP_GPS::GPS_OK_FIX_3D; + state.num_sats = 15; + + state.location = Location{ + int32_t(latitude*1e7), + int32_t(longitude*1e7), + int32_t(altitude*100), + Location::AltFrame::ABSOLUTE + }; + + state.hdop = 100; + state.vdop = 100; + + state.have_vertical_velocity = true; + state.velocity.x = speedN; + state.velocity.y = speedE; + state.velocity.z = speedD; + + state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); + state.ground_speed = state.velocity.xy().length(); + + state.have_speed_accuracy = true; + state.have_horizontal_accuracy = true; + state.have_vertical_accuracy = true; + state.have_vertical_velocity = true; + + // state.horizontal_accuracy = pkt.horizontal_pos_accuracy; + // state.vertical_accuracy = pkt.vertical_pos_accuracy; + // state.speed_accuracy = pkt.horizontal_vel_accuracy; + + state.last_gps_time_ms = now; + + return true; +} + +#endif // HAL_SIM_GPS_ENABLED diff --git a/libraries/AP_GPS/AP_GPS_SITL.h b/libraries/AP_GPS/AP_GPS_SITL.h new file mode 100644 index 0000000000..52079df1a0 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_SITL.h @@ -0,0 +1,40 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +#include "GPS_Backend.h" + +#include + +#if HAL_SIM_GPS_ENABLED + +class AP_GPS_SITL : public AP_GPS_Backend +{ + +public: + + using AP_GPS_Backend::AP_GPS_Backend; + + bool read() override; + + const char *name() const override { return "SITL"; } + +private: + + uint32_t last_update_ms; +}; + +#endif // HAL_SIM_GPS_ENABLED