ardupilot/libraries/SITL/SIM_GPS.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

404 lines
11 KiB
C++
Raw Normal View History

/*
SITL handling
This simulates a GPS on a serial port
Andrew Tridgell November 2011
*/
#include "SIM_GPS.h"
#if HAL_SIM_GPS_ENABLED
#include <time.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_HAL/AP_HAL.h>
#include <SITL/SITL.h>
#include <AP_InternalError/AP_InternalError.h>
#include "SIM_GPS_FILE.h"
#include "SIM_GPS_Trimble.h"
#include "SIM_GPS_MSP.h"
#include "SIM_GPS_NMEA.h"
#include "SIM_GPS_NOVA.h"
#include "SIM_GPS_SBP2.h"
#include "SIM_GPS_SBP.h"
#include "SIM_GPS_UBLOX.h"
// the number of GPS leap seconds - copied from AP_GPS.h
#define GPS_LEAPSECONDS_MILLIS 18000ULL
2015-05-04 04:25:58 -03:00
extern const AP_HAL::HAL& hal;
using namespace SITL;
2023-08-09 01:36:44 -03:00
// ensure the backend we have allocated matches the one that's configured:
GPS_Backend::GPS_Backend(GPS &_front, uint8_t _instance)
: front{_front},
instance{_instance}
{
_sitl = AP::sitl();
2023-08-09 01:36:44 -03:00
}
ssize_t GPS_Backend::write_to_autopilot(const char *p, size_t size) const
{
return front.write_to_autopilot(p, size);
}
ssize_t GPS_Backend::read_from_autopilot(char *buffer, size_t size) const
{
return front.read_from_autopilot(buffer, size);
}
GPS::GPS(uint8_t _instance) :
SerialDevice(8192, 2048),
instance{_instance}
{
2013-12-21 07:26:30 -04:00
}
uint32_t GPS::device_baud() const
{
2023-08-09 01:36:44 -03:00
if (backend == nullptr) {
return 0;
}
2023-08-09 01:36:44 -03:00
return backend->device_baud();
}
/*
write some bytes from the simulated GPS
*/
ssize_t GPS::write_to_autopilot(const char *p, size_t size) const
{
// the second GPS instance fails in a different way to the first;
// the first will start sending back 3 satellites, the second just
// stops responding when disabled. This is not necessarily a good
// thing.
2020-07-07 20:53:01 -03:00
if (instance == 1 && _sitl->gps_disable[instance]) {
return -1;
}
const float byteloss = _sitl->gps_byteloss[instance];
// shortcut if we're not doing byteloss:
if (!is_positive(byteloss)) {
return SerialDevice::write_to_autopilot(p, size);
}
size_t ret = 0;
while (size--) {
float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
if (r < byteloss) {
// lose the byte
p++;
continue;
}
const ssize_t pret = SerialDevice::write_to_autopilot(p, 1);
if (pret == 0) {
// no space?
return ret;
}
if (pret != 1) {
// error has occured?
return pret;
2014-03-02 16:07:29 -04:00
}
ret++;
2013-12-21 07:26:30 -04:00
p++;
}
return ret;
}
/*
get timeval using simulation time
*/
void GPS_Backend::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;
2020-09-04 19:23:51 -03:00
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;
}
/*
return GPS time of week
*/
GPS_Backend::GPS_TOW GPS_Backend::gps_time()
{
GPS_TOW gps_tow;
struct timeval tv;
simulation_timeval(&tv);
2017-01-10 05:59:02 -04:00
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;
gps_tow.week = epoch_seconds / AP_SEC_PER_WEEK;
uint32_t t_ms = tv.tv_usec / 1000;
// round time to nearest 200ms
gps_tow.ms = (epoch_seconds % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC + ((t_ms/200) * 200);
return gps_tow;
}
2023-08-09 01:36:44 -03:00
void GPS::check_backend_allocation()
{
const Type configured_type = Type(_sitl->gps_type[instance].get());
if (allocated_type == configured_type) {
return;
}
// mismatch; delete any already-allocated backend:
if (backend != nullptr) {
delete backend;
backend = nullptr;
}
// attempt to allocate backend
switch (configured_type) {
case Type::NONE:
// no GPS attached
break;
#if AP_SIM_GPS_UBLOX_ENABLED
2023-08-09 01:36:44 -03:00
case Type::UBLOX:
backend = new GPS_UBlox(*this, instance);
break;
#endif
2023-08-09 01:36:44 -03:00
#if AP_SIM_GPS_NMEA_ENABLED
2023-08-09 01:36:44 -03:00
case Type::NMEA:
backend = new GPS_NMEA(*this, instance);
break;
#endif
2023-08-09 01:36:44 -03:00
#if AP_SIM_GPS_SBP_ENABLED
2023-08-09 01:36:44 -03:00
case Type::SBP:
backend = new GPS_SBP(*this, instance);
break;
#endif
2023-08-09 01:36:44 -03:00
#if AP_SIM_GPS_SBP2_ENABLED
2023-08-09 01:36:44 -03:00
case Type::SBP2:
backend = new GPS_SBP2(*this, instance);
break;
#endif
2023-08-09 01:36:44 -03:00
#if AP_SIM_GPS_NOVA_ENABLED
2023-08-09 01:36:44 -03:00
case Type::NOVA:
backend = new GPS_NOVA(*this, instance);
break;
#endif
2023-08-09 01:36:44 -03:00
#if AP_SIM_GPS_MSP_ENABLED
2023-08-09 01:36:44 -03:00
case Type::MSP:
backend = new GPS_MSP(*this, instance);
break;
#endif
2023-08-09 01:36:44 -03:00
#if AP_SIM_GPS_TRIMBLE_ENABLED
case Type::TRIMBLE:
backend = new GPS_Trimble(*this, instance);
2023-08-09 01:36:44 -03:00
break;
#endif
2023-08-09 01:36:44 -03:00
#if AP_SIM_GPS_FILE_ENABLED
case Type::FILE:
backend = new GPS_FILE(*this, instance);
break;
#endif
};
allocated_type = configured_type;
}
/*
possibly send a new GPS packet
*/
void GPS::update()
{
if (!init_sitl_pointer()) {
return;
}
2023-08-09 01:36:44 -03:00
check_backend_allocation();
if (backend == nullptr) {
return;
}
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 uint32_t now_ms = AP_HAL::millis();
if (now_ms < 20000) {
// apply the init offsets for the first 20s. This allows for
// having the origin a long way from the takeoff location,
// which makes testing long flights easier
latitude += _sitl->gps_init_lat_ofs;
longitude += _sitl->gps_init_lon_ofs;
altitude += _sitl->gps_init_alt_ofs;
}
//Capture current position as basestation location for
2020-07-07 20:53:01 -03:00
if (!_gps_has_basestation_position &&
now_ms >= _sitl->gps_lock_time[0]*1000UL) {
2020-07-07 20:53:01 -03:00
_gps_basestation_data.latitude = latitude;
_gps_basestation_data.longitude = longitude;
_gps_basestation_data.altitude = altitude;
_gps_basestation_data.speedN = speedN;
_gps_basestation_data.speedE = speedE;
_gps_basestation_data.speedD = speedD;
_gps_has_basestation_position = true;
}
const uint8_t idx = instance; // alias to avoid code churn
2020-07-07 20:53:01 -03:00
struct GPS_Data d {};
// simulate delayed lock times
bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL);
// Only let physics run and GPS write at configured GPS rate (default 5Hz).
if ((now_ms - last_write_update_ms) < (uint32_t)(1000/_sitl->gps_hertz[instance])) {
// Reading runs every iteration.
// Beware- physics don't update every iteration with this approach.
// Currently, none of the drivers rely on quickly updated physics.
backend->update_read();
return;
}
last_write_update_ms = now_ms;
d.latitude = latitude;
d.longitude = longitude;
d.yaw_deg = _sitl->state.yawDeg;
d.roll_deg = _sitl->state.rollDeg;
d.pitch_deg = _sitl->state.pitchDeg;
// add an altitude error controlled by a slow sine wave
d.altitude = altitude + _sitl->gps_noise[idx] * sinf(now_ms * 0.0005f) + _sitl->gps_alt_offset[idx];
// Add offset to c.g. velocity to get velocity at antenna and add simulated error
Vector3f velErrorNED = _sitl->gps_vel_err[idx];
d.speedN = speedN + (velErrorNED.x * rand_float());
d.speedE = speedE + (velErrorNED.y * rand_float());
d.speedD = speedD + (velErrorNED.z * rand_float());
d.have_lock = have_lock;
if (_sitl->gps_drift_alt[idx] > 0) {
// add slow altitude drift controlled by a slow sine wave
d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f);
}
// correct the latitude, longitude, height and NED velocity for the offset between
// the vehicle c.g. and GPS antenna
Vector3f posRelOffsetBF = _sitl->gps_pos_offset[idx];
if (!posRelOffsetBF.is_zero()) {
// get a rotation matrix following DCM conventions (body to earth)
Matrix3f rotmat;
_sitl->state.quaternion.rotation_matrix(rotmat);
// rotate the antenna offset into the earth frame
Vector3f posRelOffsetEF = rotmat * posRelOffsetBF;
// Add the offset to the latitude, longitude and height using a spherical earth approximation
double const earth_rad_inv = 1.569612305760477e-7; // use Authalic/Volumetric radius
double lng_scale_factor = earth_rad_inv / cos(radians(d.latitude));
d.latitude += degrees(posRelOffsetEF.x * earth_rad_inv);
d.longitude += degrees(posRelOffsetEF.y * lng_scale_factor);
d.altitude -= posRelOffsetEF.z;
// calculate a velocity offset due to the antenna position offset and body rotation rate
// note: % operator is overloaded for cross product
Vector3f gyro(radians(_sitl->state.rollRate),
radians(_sitl->state.pitchRate),
radians(_sitl->state.yawRate));
Vector3f velRelOffsetBF = gyro % posRelOffsetBF;
// rotate the velocity offset into earth frame and add to the c.g. velocity
Vector3f velRelOffsetEF = rotmat * velRelOffsetBF;
d.speedN += velRelOffsetEF.x;
d.speedE += velRelOffsetEF.y;
d.speedD += velRelOffsetEF.z;
}
// get delayed data
d.timestamp_ms = now_ms;
d = interpolate_data(d, _sitl->gps_delay_ms[instance]);
// Applying GPS glitch
// Using first gps glitch
Vector3f glitch_offsets = _sitl->gps_glitch[idx];
d.latitude += glitch_offsets.x;
d.longitude += glitch_offsets.y;
d.altitude += glitch_offsets.z;
backend->publish(&d);
2023-08-09 01:36:44 -03:00
}
void GPS_Backend::update_read()
2023-08-09 01:36:44 -03:00
{
// swallow any config bytes
char c;
read_from_autopilot(&c, 1);
}
/*
get delayed data by interpolation
*/
2023-08-09 01:36:44 -03:00
GPS_Data GPS::interpolate_data(const GPS_Data &d, uint32_t delay_ms)
{
const uint8_t N = ARRAY_SIZE(_gps_history);
const uint32_t now_ms = d.timestamp_ms;
// add in into history array, shifting old elements
memmove(&_gps_history[1], &_gps_history[0], sizeof(_gps_history[0])*(ARRAY_SIZE(_gps_history)-1));
_gps_history[0] = d;
for (uint8_t i=0; i<N-1; i++) {
uint32_t dt1 = now_ms - _gps_history[i].timestamp_ms;
uint32_t dt2 = now_ms - _gps_history[i+1].timestamp_ms;
if (delay_ms >= dt1 && delay_ms <= dt2) {
// we will interpolate this pair of samples. Start with
// the older sample
2023-08-09 01:36:44 -03:00
const GPS_Data &s1 = _gps_history[i+1];
const GPS_Data &s2 = _gps_history[i];
GPS_Data d2 = s1;
const float p = (dt2 - delay_ms) / MAX(1,float(dt2 - dt1));
d2.latitude += p * (s2.latitude - s1.latitude);
d2.longitude += p * (s2.longitude - s1.longitude);
d2.altitude += p * (s2.altitude - s1.altitude);
d2.speedN += p * (s2.speedN - s1.speedN);
d2.speedE += p * (s2.speedE - s1.speedE);
d2.speedD += p * (s2.speedD - s1.speedD);
2023-07-30 22:40:35 -03:00
d2.yaw_deg += p * wrap_180(s2.yaw_deg - s1.yaw_deg);
return d2;
}
}
// delay is too long, use last sample
return _gps_history[N-1];
}
2023-08-09 01:36:44 -03:00
float GPS_Data::heading() const
{
const auto velocity = Vector2d{speedE, speedN};
return velocity.angle();
}
2023-08-09 01:36:44 -03:00
float GPS_Data::speed_2d() const
{
const auto velocity = Vector2d{speedN, speedE};
return velocity.length();
}
#endif // HAL_SIM_GPS_ENABLED